#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import waymo_open_dataset
import os.path, pkgutil
import tensorflow as tf
import math 
import time

def measure_time(func):
    def wrapper(*args, **kwargs):
        start_time = time.time()
        result = func(*args, **kwargs)
        end_time = time.time()
        execution_time = end_time - start_time
        print(f"函数 {func.__name__} 运行时间：{execution_time:.6f} 秒")
        return result
    return wrapper
pkgpath = os.path.dirname(waymo_open_dataset.__file__)
print([name for _, name, _ in pkgutil.iter_modules([pkgpath])])
from tqdm import tqdm

from waymo_open_dataset.metrics.ops import py_metrics_ops
from waymo_open_dataset.metrics.python import config_util_py as config_util
from waymo_open_dataset.protos import scenario_pb2 
from waymo_open_dataset.protos import map_pb2
from waymo_open_dataset.metrics.ops import py_metrics_ops
from waymo_open_dataset.metrics.python import config_util_py as config_util
from scipy.interpolate import CubicSpline
#from waymo_open_dataset.protos import motion_submission

from google.protobuf import text_format
from .waymo_scenario_classes import *
from .waymo_training_plot_tools import *
from .waymo_training_math_tools import *

# process begin
def process_waymo_training_dataset(waymo_dataset_file, scenarios_to_plot_list):
    motion_dataset = tf.data.TFRecordDataset(waymo_dataset_file)
    motion_scenario_data_list = []
    motion_lanecenter_data_list = []
    # read data from proto
    for motion_data in motion_dataset:
        proto_string = motion_data.numpy()
        scenario_proto = scenario_pb2.Scenario()
        scenario_proto.ParseFromString(proto_string)
        # print(type(scenario_proto))
        motion_scenario_data_list.append(scenario_proto)    
        # lane_proto = map_pb2.LaneCenter()
        # lane_proto.ParseFromString(proto_string)
        # #print(type(lane_proto))
        # motion_lanecenter_data_list.append(lane_proto)
    scenarios_summary = Scenarios()
    scenarios_summary.scenarios_file_name = waymo_dataset_file.split("\\")[-1]
    # lane_center_summary = LaneCenters()
    # lane_center_summary.lanecenter_file_name = waymo_dataset_file.split("\\")[-1]
    # motion_scenario_data is a list, only has length
    #print("scenarios number in this dataset: ", len(motion_scenario_data_list))
    # print("LaneCenter data number in this dataset: ", len(motion_lanecenter_data_list))
    # lanecenter_number = 0
    # iterate every scenario
    scenario_number = 0
    #scenarios_to_plot_list = [0,100,200,300]
    for motion_scenario in motion_scenario_data_list:
        if scenario_number not in scenarios_to_plot_list:
            scenario_number += 1
            continue
        
        print("motion_scenario_id: ")
        print(motion_scenario.scenario_id)
        
        # print("sdc_track_index: ")
        # print(motion_scenario.sdc_track_index)
        
        # print("number of map features: ")
        # print(len(motion_scenario.map_features))
        
        # print("number of agent tracks: ")
        # print(len(motion_scenario.tracks))    
        
        scenario = Motion_training_scenario()
        scenario.scenario_id = motion_scenario.scenario_id
        scenario.timestep_seconds_list = motion_scenario.timestamps_seconds
        scenario.timestep_list = [i for i in range(len(scenario.timestep_seconds_list))]
        scenario.current_time_index = motion_scenario.current_time_index
        scenario.sdc_track_index = motion_scenario.sdc_track_index
        scenario.number_of_map_features = len(motion_scenario.map_features)
        scenario.tracks_to_pre_list = motion_scenario.tracks_to_predict   

        i = 0
        lane_flist = []
        road_edge_flist = []
        road_line_flist = []
        stop_sign_flist = []
        crosswalk_flist = []
        speed_bump_flist = []
        timesteps_num = 0
        scenario.dynamic_map_states = []
        for dynamic_states in motion_scenario.dynamic_map_states:
            timesteps_num = 1

            scenario.dynamic_map_states.append(dynamic_states.lane_states)



        for map_feature in motion_scenario.map_features:
            
            feature_id = map_feature.id
            road_line_type = map_feature.WhichOneof('feature_data')
            
            
            
            
            if road_line_type == "lane": # center line    
                i += 1               
                center_lane = Center_lane()
                center_lane.lane_id = feature_id
                #print("center lane id is: ",feature_id)
                center_lane.lane_type = "lane"            
                center_lane.speed_limit = map_feature.lane.speed_limit_mph
                
                # for vehicle map matching
                center_lane_position_list = []
                
                ccc = []
                ddd = []
                eee = []
                fff = []
                ggg = []
                hhh = []
                center_lane.left_neighbors_id_list = []
                center_lane.right_neighbors_id_list = []
                center_lane.entry_lane_id_list = []
                center_lane.exit_lane_id_list = []
                center_lane.left_neighbors_list = []
                center_lane.right_neighbors_list = []
                center_lane.left_boundary_list = []
                center_lane.right_boundary_list = []
                for neighbor_lane in map_feature.lane.left_neighbors:
                    ccc.append(neighbor_lane.feature_id)
                    center_lane.left_neighbors_list.append(neighbor_lane)
                    center_lane.left_neighbors_id_list.append(neighbor_lane.feature_id)
                if ccc == []:
                    ccc.append(None)
                center_lane.left_neighbors_id_dict.update({feature_id:ccc})
                    
                for neighbor_lane in map_feature.lane.right_neighbors: 
                    ddd.append(neighbor_lane.feature_id)
                    center_lane.right_neighbors_list.append(neighbor_lane)
                    center_lane.right_neighbors_id_list.append(neighbor_lane.feature_id)
                if ddd == []:
                    ddd.append(None)
                center_lane.right_neighbors_id_dict.update({feature_id:ddd})
                    
                for entry_lane in map_feature.lane.entry_lanes: 
                    center_lane.entry_lane_id_list.append(entry_lane)
                    ggg.append(entry_lane)
                if ggg == []:
                    ggg.append(None)
                center_lane.entry_lane_id_dict.update({feature_id:ggg})                    
                    
                for exit_lane in map_feature.lane.exit_lanes: 
                    center_lane.exit_lane_id_list.append(exit_lane)   
                    hhh.append(exit_lane)
                if hhh == []:
                    hhh.append(None)
                center_lane.exit_lane_id_dict.update({feature_id:hhh})  
                    
                # for center in map_feature.lane.center_lane:
                #     center_lane.center_points_list.append(center)
                    
                for line in map_feature.lane.left_boundaries:
                    if line is not None:
                        center_lane.left_boundary_list.append(line)
                        eee.append(line)
                center_lane.left_boundary_segment_dict.update({feature_id:eee})
                    
                for line in map_feature.lane.right_boundaries:
                    if line is not None:
                        center_lane.right_boundary_list.append(line)
                        fff.append(line)
                center_lane.right_boundary_segment_dict.update({feature_id:fff})
                
                aaa = []
                bbb = []
                center_lane.point_x_list = []
                center_lane.point_y_list = []
                #print(dir(map_feature.lane.polyline))
                for point in map_feature.lane.polyline:
                    center_lane.point_x_list.append(point.x)
                    center_lane.point_y_list.append(point.y)
                    center_lane.points_list.append((point.x, point.y))
                    aaa.append(point.x)
                    bbb.append(point.y)
                scenario.map_features_list.append(center_lane)
                scenario.center_lane_id_dict.update({feature_id:[aaa, bbb]})
                scenario.lane_black_list.extend([center_lane.point_x_list, center_lane.point_y_list])
                scenario.center_lane_list.append(center_lane)
                scenario.center_lane_id_list.append(feature_id)
                scenario.center_lane_dict[feature_id] = center_lane
                lane_flist.append(feature_id)

                
            
            if road_line_type == "road_edge":        
                road_edge = Road_edge()
                edge_px = []
                edge_py = []
                road_edge.line_id = feature_id
                road_edge.line_type = "road_edge"
                for point in map_feature.road_edge.polyline:
                    edge_px.append(point.x)
                    edge_py.append(point.y)
                    road_edge.point_x_list.append(point.x)
                    road_edge.point_y_list.append(point.y)   
                scenario.map_features_list.append(road_edge)
                scenario.road_edge_list.extend([road_edge.point_x_list, road_edge.point_y_list])
                road_edge_flist.append(feature_id)
                   
                scenario.road_edge_dict.update({feature_id:[edge_px,edge_py]})
                scenario.road_edge_dict_tuple.update({feature_id:list_to_tuple([edge_px,edge_py])})
            if road_line_type == "road_line":          
                white_px = []
                white_py = []
                passing_px = []
                passing_py = []
                is_yellow = False
                road_line = Road_line()
                road_line.line_id = feature_id
                road_line.line_type = "road_line"          
                for point in map_feature.road_line.polyline:
                    if map_feature.road_line.type == map_feature.road_line.RoadLineType.TYPE_UNKNOWN:
                        unknown_x_list.append(point.x)
                        unknown_y_list.append(point.y)
                        
                    # white line lists
                    if map_feature.road_line.type == map_feature.road_line.RoadLineType.TYPE_BROKEN_SINGLE_WHITE:
                        white_px.append(point.x)
                        white_py.append(point.y)
                        road_line.broken_single_white_x_list.append(point.x)
                        road_line.broken_single_white_y_list.append(point.y)                
                    if map_feature.road_line.type == map_feature.road_line.RoadLineType.TYPE_SOLID_SINGLE_WHITE:
                        white_px.append(point.x)
                        white_py.append(point.y)
                        road_line.solid_single_white_x_list.append(point.x)
                        road_line.solid_single_white_y_list.append(point.y)  
                    if map_feature.road_line.type == map_feature.road_line.RoadLineType.TYPE_SOLID_DOUBLE_WHITE:
                        white_px.append(point.x)
                        white_py.append(point.y)
                        road_line.solid_double_white_x_list.append(point.x)
                        road_line.solid_double_white_y_list.append(point.y)   
                        
                    # yellow line lists
                    if map_feature.road_line.type == map_feature.road_line.RoadLineType.TYPE_BROKEN_SINGLE_YELLOW:
                        road_line.broken_single_yellow_x_list.append(point.x)
                        road_line.broken_single_yellow_y_list.append(point.y)                       
                        
                    if map_feature.road_line.type == map_feature.road_line.RoadLineType.TYPE_BROKEN_DOUBLE_YELLOW:
                        road_line.broken_double_yellow_x_list.append(point.x)
                        road_line.broken_double_yellow_y_list.append(point.y)                       
                        
                    if map_feature.road_line.type == map_feature.road_line.RoadLineType.TYPE_SOLID_SINGLE_YELLOW:
                        road_line.solid_single_yellow_x_list.append(point.x)
                        road_line.solid_single_yellow_y_list.append(point.y)   
    
                    if map_feature.road_line.type == map_feature.road_line.RoadLineType.TYPE_SOLID_DOUBLE_YELLOW:
                        road_line.solid_double_yellow_x_list.append(point.x)
                        road_line.solid_double_yellow_y_list.append(point.y)    
                        
                    if map_feature.road_line.type == map_feature.road_line.RoadLineType.TYPE_PASSING_DOUBLE_YELLOW:
                        is_yellow = True
                        passing_px.append(point.x)
                        passing_py.append(point.y)
                        road_line.passing_double_yellow_x_list.append(point.x)
                        road_line.passing_double_yellow_y_list.append(point.y)                        
                                            
                    # all points lists    
                    road_line.point_x_list.append(point.x)
                    road_line.point_y_list.append(point.y)  
    
                # white line lists
                scenario.broken_single_white_list.extend([road_line.broken_single_white_x_list, road_line.broken_single_white_y_list])
                scenario.solid_single_white_list.extend([road_line.solid_single_white_x_list, road_line.solid_single_white_y_list])
                scenario.solid_double_white_list.extend([road_line.solid_double_white_x_list, road_line.solid_double_white_y_list])

                scenario.plot_short_line_list.append([road_line.broken_single_white_x_list, road_line.broken_single_white_y_list])
                scenario.plot_short_line_list.append([road_line.solid_single_white_x_list, road_line.solid_single_white_y_list])
                scenario.plot_short_line_list.append([road_line.solid_double_white_x_list, road_line.solid_double_white_y_list])
                
                # yellow line lists    
                scenario.broken_single_yellow_list.extend([road_line.broken_single_yellow_x_list, road_line.broken_single_yellow_y_list])
                scenario.broken_double_yellow_list.extend([road_line.broken_double_yellow_x_list, road_line.broken_double_yellow_y_list])
                scenario.solid_single_yellow_list.extend([road_line.solid_single_yellow_x_list, road_line.solid_single_yellow_y_list])
                scenario.solid_double_yellow_list.extend([road_line.solid_double_yellow_x_list, road_line.solid_double_yellow_y_list])
                scenario.passing_double_yellow_list.extend([road_line.passing_double_yellow_x_list, road_line.passing_double_yellow_y_list])      
                
                scenario.plot_short_line_list.append([road_line.broken_single_yellow_x_list, road_line.broken_single_yellow_y_list])
                scenario.plot_short_line_list.append([road_line.broken_double_yellow_x_list, road_line.broken_double_yellow_y_list])
                scenario.plot_short_line_list.append([road_line.solid_single_yellow_x_list, road_line.solid_single_yellow_y_list])
                scenario.plot_short_line_list.append([road_line.solid_double_yellow_x_list, road_line.solid_double_yellow_y_list])
                scenario.plot_short_line_list.append([road_line.passing_double_yellow_x_list, road_line.passing_double_yellow_y_list])
                        
                scenario.map_features_list.append(road_line) 
                road_line_flist.append(feature_id)
                
                
                scenario.white_lane_dict.update({feature_id:[white_px,white_py]})
                if is_yellow == True:
                    scenario.passing_yellow_lane_dict.update({feature_id:[passing_px,passing_py]})
                
                
               
                
                       
            if road_line_type == "stop_sign":        
                stop_sign = Stop_sign()
                stop_sign.stop_sign_id = feature_id
                stop_sign.lstop_sign_type = "stop_sign"
                stop_sign.conrtolled_lane_id = map_feature.stop_sign.lane
                
                point = map_feature.stop_sign.position
                stop_sign.point_x_list.append(point.x)
                stop_sign.point_y_list.append(point.y)
                scenario.stop_sign_list.extend([stop_sign.point_x_list, stop_sign.point_y_list])              
                scenario.map_features_list.append(stop_sign)
                stop_sign_flist.append(feature_id)
                
                                               
            if road_line_type == "crosswalk":      
                crosswalk = Cross_walk()                
                for point in map_feature.crosswalk.polygon:
                    crosswalk.point_x_list.append(point.x)
                    crosswalk.point_y_list.append(point.y)                   
                scenario.cross_walk_list.append(crosswalk)
                scenario.map_features_list.append(crosswalk)
                crosswalk_flist.append(feature_id)
                scenario.all_feature_ids.update({"crosswalk":feature_id})
                
            if road_line_type == "speed_bump":               
                speed_bump = Speed_bump()
                for point in map_feature.speed_bump.polygon:
                    speed_bump.point_x_list.append(point.x)
                    speed_bump.point_y_list.append(point.y)   
                scenario.map_features_list.append(speed_bump) 
                speed_bump_flist.append(feature_id)
        
        # iterator tracks/agents
        track_index = 0
        for track in motion_scenario.tracks:
            agent = Motion_training_agent()
            agent.agent_id = track.id
            agent.agent_type = track.object_type
            

            if track_index == scenario.sdc_track_index:
                agent.agent_type = -1
            if agent.agent_type == 1 or agent.agent_type == -1:
                scenario.car_id_list.append(agent.agent_id)
                
            
            state_index = 0
            states_array = np.empty((0,14), dtype = float)
            first_x = 0.0
            first_y = 0.0
            cishu = 0
            first_app_time = 0
            for track_state in track.states:
                state = Agent_states()
                state.timestep = scenario.timestep_list[state_index]
                state.agent_id = track.id  

                state.agent_type = agent.agent_type
                state.center_x = track_state.center_x
                state.center_y = track_state.center_y 
                if cishu == 0:
                    if (track_state.center_x != 0.0) and (track_state.center_y != 0.0):
                        first_x = track_state.center_x
                        first_y = track_state.center_y
                        agent.first_appear_time = first_app_time
                        cishu += 10
                first_app_time += 1
                state.center_z = track_state.center_z 
                state.length   = track_state.length
                state.width    = track_state.width    
                state.height   = track_state.height

                state.velocity_x = track_state.velocity_x
                state.velocity_y = track_state.velocity_y
                state.speed = math.sqrt(track_state.velocity_x**2+track_state.velocity_y**2) #12
                state.valid = track_state.valid           #13
                a = track_state.heading
                # while a < -math.pi:
                #     a += math.pi
                # while a > math.pi:
                #     a -= math.pi
                state.heading  = a
                single_state = np.array([[state.timestep, state.agent_id, state.agent_type, state.center_x, state.center_y, state.center_z, state.length, state.width, state.height, state.heading, state.velocity_x, state.velocity_y, state.speed, state.valid]])          
                
                states_array = np.append(states_array, single_state, axis=0) 
                             
                agent.states_list.append(state)
                agent.states_mask_list.append(state.valid)
                if state.valid:
                    scenario.all_agents_x_list.append(state.center_x)
                    scenario.all_agents_y_list.append(state.center_y)
                state_index += 1
            agent.states_array = states_array
            if (first_x != 0.0 ) and (first_y != 0.0):
                current_lane_id, left_n_list, right_n_list, entry_l, exit_l = initial_lane_of_track(scenario.center_lane_id_dict, scenario.center_lane_dict, first_x, first_y)
            else:
                    print("Initialize which lane the track in failed(first two x,y is error)")
            agent.agent_ini_lane_id_list = [current_lane_id, left_n_list, right_n_list ,entry_l,exit_l]
            scenario.agents_list.append(agent)
            
            
            
            track_index += 1     
        for state in scenario.dynamic_map_states:
            for lane_info in state:
                lane_id = lane_info.lane
                stop_sign = lane_info.state
                stop_point = lane_info.stop_point
                if stop_sign == 0:
                    scenario.center_lane_dict[lane_id].stop_sign_list.append(None)
                if stop_sign == 2 or stop_sign == 5 or stop_sign == 8:
                    scenario.center_lane_dict[lane_id].stop_sign_list.append(2)
                if stop_sign == 3 or stop_sign == 6:
                    scenario.center_lane_dict[lane_id].stop_sign_list.append(1)
                if stop_sign == 1 or stop_sign == 4 or stop_sign == 7:
                    scenario.center_lane_dict[lane_id].stop_sign_list.append(0)
                scenario.center_lane_dict[lane_id].stop_point = stop_point
                
           
        # for Lanecenter in motion_centerlane_data_list:
        scenario.all_feature_ids.update({"speed_bump":speed_bump_flist})
        scenario.all_feature_ids.update({"stop_sign":stop_sign_flist})     
        scenario.all_feature_ids.update({"road_line":road_line_flist}) 
        scenario.all_feature_ids.update({"road_edge":road_edge_flist}) 
        scenario.all_feature_ids.update({"lane":lane_flist})
        scenarios_summary.scenarios_list.append(scenario)                
        scenario_number += 1
    return scenarios_summary



END_STEP = 91
class Dataloader:
    def __init__(self, data_path, num_scenarios = 1):
        self.scenario_list = process_waymo_training_dataset(data_path, [0])
        self.scenario = self.scenario_list.scenarios_list[0]
        self.get_agents_id_to_idx_dict()
        self.is_this_car_hold()
        self.is_this_lane_straight()
        self.get_agent_all_time_lane_list()
        self.get_all_time_cars_in_lane()
        self.get_cars_around_agents()
        self.get_distance_to_around_lane()
        self.get_agents_to_pre()
        self.is_lane_crossroad()

    def get_scenario(self, scenario_index = 0):
        # return self.scenario_list.scenario_list[scenario_index]
        return self.scenario
    def get_plot_line_list(self):
        return self.scenario.plot_short_line_list
    def get_agents_id_to_idx_dict(self):
        all_agents = self.scenario.agents_list
        id_dict ={}
        idlist = []
        for idx, agent in enumerate(all_agents):
            agent_id = agent.agent_id
            idlist.append(agent_id)
            id_dict.update({agent_id:idx})
        self.scenario.agents_id_to_idx_dict = id_dict
        return True
    def get_all_agents_id(self):
        iddict = self.scenario.agents_id_to_idx_dict
        idlist = list(iddict.keys())
        return idlist
    def get_time_index_list(self):
        return self.scenario.timestep_list
    
    def get_scenario_id(self,scenario_index = 0):
        return self.scenario_list.scenario_list[0]
    
    def get_center_lane_dict(self):
        return self.scenario.center_lane_dict
    
    def get_current_time_index(self):
        return self.scenario.current_time_index
    
    def get_all_agents_x_list(self):
        return self.scenario.all_agents_x_list
    
    def get_all_agents_y_list(self):
        return self.scenario.all_agents_y_list
    
    def get_map_feature_list(self):
        return self.scenario.map_features_list

    def get_number_of_map_features(self):
        return self.scenario.number_of_map_features  
    
    def get_passing_double_yellow_list(self):
        return self.scenario.passing_double_yellow_list
        
    def get_broken_double_yellow_list(self):
        return self.scenario.broken_double_yellow_list
    
    def get_broken_single_white_list(self):
        return self.scenario.broken_single_white_list
    
    def get_broken_single_yellow_list(self):
        return self.scenario.broken_single_yellow_list
    
    
    def get_center_lane_list(self):
        return self.scenario.center_lane_list
    
    def get_center_walk_list(self):
        return self.scenario.center_walk_list
    
    def get_road_edge_list(self):
        return self.scenario.road_edge_list
    
    def get_lane_black_list(self):
        return self.scenario.road_edge_list
    
    def get_stop_sign_list(self):
        return self.scenario.stop_sign_list
    
    def get_lane_center_id_dict(self):
        return self.scenario.center_lane_id_dict
    
    def get_solid_double_yellow_list(self):
        return self.scenario.solid_double_yellow_list
    
    def get_solid_double_white_list(self):
        return self.scenario.solid_double_white_list

    def get_solid_single_white_list(self):
        return self.scenario.solid_single_white_list
    
    def get_solid_single_yellow_list(self):
        return self.scenario.solid_single_yellow_list
    
    def get_stop_sign_list(self):
        return self.scenario.stop_sign_list
    
    def get_real_time_list(self,start_time_index , end_time_index):
        return self.scenario.timestep_seconds_list[start_time_index : end_time_index]
    
    def get_all_car_id_list(self):
        return self.scenario.car_id_list
    
    def get_ego_vehicle_states_array(self):
        return self.scenario.agents_list[self.scenario.sdc_track_index].states_array
    
    def get_ego_vehicle_states_list(self):
        return self.scenario.agents_list[self.scenario.sdc_track_index].states_list
    
    def get_ego_vehicle_validation_list(self):
        return self.scenario.agents_list[self.scenario.sdc_track_index].states_mask_list

    def get_all_features_ids(self):
        return self.scenario.all_feature_ids
    
    def get_ego_vehicle_id(self):
        return self.scenario.agents_list[self.scenario.sdc_track_index].agent_id
        
    def get_ego_vehicle_center_x_list(self, start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.center_x for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_center_x = interp_center_position(partial_states)
        return interped_center_x
    
    def get_ego_vehicle_center_y_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.center_y for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_center_y = interp_center_position(partial_states)
        return interped_center_y
   
    def get_ego_vehicle_center_z_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.center_z for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_heading_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.heading for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_heigth_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.height for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_width_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.width for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_length_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.length for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_speed_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.speed for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_v_x_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.velocity_x for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_v_y_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.velocity.y for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_acc_and_acctheta_list(self,start_time_index = 0, end_time_index = END_STEP):
        len = end_time_index - start_time_index
        v = [state.speed for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        vx = [state.velocity_x for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        vy = [state.velocity_y for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        time = self.scenario.timestep_seconds_list[start_time_index : end_time_index]
        acc = [0] * len
        theta_acc = [0] * len
        for i in range(1, len - 1):
            dt = time[i+1] - time[i]
            acc[i] = (v[i+1] - 2*v[i] + v[i-1]) / (dt**2)
            dx = vx[i+1] - vx[i-1]
            dy = vy[i+1] - vy[i-1]
            theta_acc[i] = math.atan2(dy, dx)
        dt_1 = time[1] - time[0]
        dt_2 = time[2] - time[1]
        acc[0] = (v[1] - v[0]) / dt_1
        acc[1] = (v[2] - v[0]) / (dt_1 + dt_2)
        theta_acc[0] = math.atan2(vy[1], vx[1])
        theta_acc[1] = math.atan2(vy[2] - vy[0], vx[2] - vx[0])
        dt_3 = time[-3] - time[-2]
        dt_4 = time[-2] - time[-1]
        acc[-2] = (v[-1] - v[-3]) / (dt_3 + dt_4)
        acc[-1] = (v[-1] - v[-2]) / dt_4
        theta_acc[-2] = math.atan2(vy[-1] - vy[-3], vx[-1] - vx[-3])
        theta_acc[-1] = math.atan2(vy[-1], vx[-1])
        return np.array(acc), np.array(theta_acc)
        
    def get_ego_vehicle_pos_list(self, start_time_index = 0, end_time_index = END_STEP):
        partial_pos = []
        for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]:
            pos = np.array((state.center_x, state.center_y, state.center_z))
            partial_pos.append(pos)
        partial_pos = np.array(partial_pos)
        return partial_pos
    
    def get_ego_vehicle_shape_list(self, start_time_index = 0, end_time_index = END_STEP):
        partial_shape = []
        for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]:
            shape = np.array((state.length, state.width, state.height))
            partial_shape.append(shape)
        partial_shape = np.array(partial_shape)
        return partial_shape    
    
    
    
    def get_front_vehicle_id_list(self, agent_id, start_time = 0, end_time = 0):
        front_vehicle_id_list = []
        for time_index in range(start_time, end_time):
            front_vehicle_id,_ = find_front_back_vehicle(self.scenario, time_index, self.scenario.agents_list[id_dict[agent_id]].agent_id)
            if front_vehicle_id is None:
                front_vehicle_id = -1
            front_vehicle_id_list.append(front_vehicle_id)
        return front_vehicle_id_list
    
    def get_back_vehicle_id_list(self, agent_id, start_time = 0, end_time = 0):
        back_vehicle_id_list = []
        for time_index in range(start_time, end_time):
            _,back_vehicle_id = find_front_back_vehicle(self.scenario, time_index, self.scenario.agents_list[id_dict[agent_id]].agent_id)
            if back_vehicle_id is None:
                back_vehicle_id = -1
            back_vehicle_id_list.append(back_vehicle_id)
        return back_vehicle_id_list
    
    
    def get_agent_states_array_byId(self, agent_id):
        id_dict = self.scenario.agents_id_to_idx_dict
        return self.scenario.agents_list[id_dict[agent_id]].states_array
    
    def get_agent_in_lanes_byId(self, agent_id):
        id_dict = self.scenario.agents_id_to_idx_dict
        return self.scenario.agents_list[id_dict[agent_id]].agent_ini_lane_id_list
    
    def get_agent_states_list_byId(self, agent_id):
        id_dict = self.scenario.agents_id_to_idx_dict
        return self.scenario.agents_list[id_dict[agent_id]].states_array
    
    def get_agent_validation_list_byId(self, agent_id):
        id_dict = self.scenario.agents_id_to_idx_dict
        return self.scenario.agents_list[id_dict[agent_id]].states_mask_list
        

        
    def get_agent_center_x_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.center_x for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_center_x = interp_center_position(partial_states)
        for idx,i in enumerate(interped_center_x):
            if  math.isnan(i) or i == 0.0:
                interped_center_x[idx] = None
        return interped_center_x
    
    def get_agent_center_y_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.center_y for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_center_y = interp_center_position(partial_states)
        for idx,i in enumerate(interped_center_y):
            if np.isnan(i) or i == 0.0:
                interped_center_y[idx] = None
        return interped_center_y 
   
    def get_agent_center_z_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.center_z for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_center_z = interp_center_position(partial_states)
        for idx,i in enumerate(interped_center_z):
            if  math.isnan(i) or i == 0.0:
                interped_center_z[idx] = None
        return interped_center_z 
    
    def get_agent_heading_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.heading for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_heading  = interp_center_position(partial_states)
        for idx,i in enumerate(interped_heading):
            if math.isnan(i) or i == 0.0:
                interped_heading[idx] = None
            else:
                interped_heading[idx] = wrap_to_pi(i)
        return interped_heading
    
    def get_agent_height_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.height for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_height = interp_center_position(partial_states)
        for idx,i in enumerate(interped_height):
            if math.isnan(i) or i == 0.0:
                interped_height[idx] = None
        return np.array(partial_states)
    
    def get_agent_width_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.width for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_width = interp_center_position(partial_states)
        for idx,i in enumerate(interped_width):
            if math.isnan(i) or i == 0.0:
                interped_width[idx] = None
        return np.array(partial_states)
    
    def get_agent_length_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.length for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_length = interp_center_position(partial_states)
        for idx,i in enumerate(interped_length):
            if math.isnan(i) or i == 0.0:
                interped_length[idx] = None
        return np.array(partial_states)
    
    def get_agent_speed_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.speed for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_speed  = interp_center_position(partial_states)
        for idx,i in enumerate(interped_speed):
            if math.isnan(i) or i == 0.0:
                interped_speed[idx] = None
        return np.array(interped_speed)
    
    def get_agent_v_x_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.velocity_x for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_vx = interp_center_position(partial_states)
        for idx,i in enumerate(interped_vx):
            if  math.isnan(i) or i == 0.0:
                interped_vx[idx] = None
        return interped_vx 
    
    def get_agent_v_y_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.velocity_y for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_center_vy = interp_center_position(partial_states)
        for idx,i in enumerate(interped_center_vy):
            if math.isnan(i) or i == 0.0:
                interped_center_vy[idx] = None
        return interped_center_vy
    
    def get_agent_acc_and_acctheta_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        length = end_time_index - start_time_index
        v = self.get_agent_speed_list_byId(agent_id)[start_time_index:end_time_index]
        vx = self.get_agent_v_x_list_byId(agent_id)[start_time_index:end_time_index]
        vy = self.get_agent_v_x_list_byId(agent_id)[start_time_index:end_time_index]
        time = self.scenario.timestep_seconds_list[start_time_index : end_time_index]
        acc = [0] * length
        theta_acc = [0] * length
        for i in range(1, length - 1):
            dt = 0.1
            acc[i] = (v[i]-v[i-1]) / (dt)
            dx = vx[i+1] - vx[i-1]
            dy = vy[i+1] - vy[i-1]
            theta_acc[i] = math.atan2(dy, dx)
        dt_1 = time[1] - time[0]
        dt_2 = time[2] - time[1]
        acc[0] = (v[1] - v[0]) / dt_1
        #acc[1] = (v[2] - v[0]) / (dt_1 + dt_2)
        theta_acc[0] = math.atan2(vy[1], vx[1])
        theta_acc[1] = math.atan2(vy[2] - vy[0], vx[2] - vx[0])
        dt_3 = time[-3] - time[-2]
        dt_4 = time[-2] - time[-1]
        #acc[-2] = (v[-1] - v[-3]) / (dt_3 + dt_4)
        acc[-1] = (v[-1] - v[-2]) / dt_4
        theta_acc[-2] = math.atan2(vy[-1] - vy[-3], vx[-1] - vx[-3])
        theta_acc[-1] = math.atan2(vy[-1], vx[-1])
        return np.array(acc), np.array(theta_acc)
    
    def get_agent_pos_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        partial_pos = []
        id_dict = self.scenario.agents_id_to_idx_dict
        for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]:
            pos = np.array((state.center_x, state.center_y, state.center_z))
            partial_pos.append(pos)
        partial_pos = np.array(partial_pos)
        return partial_pos
    
    def get_agent_shape_list_byId(self, start_time_index = 0, end_time_index = END_STEP):
        partial_shape = []
        id_dict = self.scenario.agents_id_to_idx_dict
        for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]:
            shape = np.array((state.length, state.width, state.height))
            partial_shape.append(shape)
        partial_shape = np.array(partial_shape)
        return partial_shape

    
    
    def get_lane_boundary_byID(self,lane_id):
        left_b_dict = {}
        right_b_dict = {}
        lane = self.scenario.center_lane_dict
        center_x = lane[lane_id].point_x_list
        center_y = lane[lane_id].point_y_list
        left_boundary_list = lane[lane_id].left_boundary_list
        right_boundary_list = lane[lane_id].right_boundary_list
        white_lane_dict = self.scenario.white_lane_dict
        white_lane_id_list = list(white_lane_dict.keys())
        road_edge_dict = self.scenario.road_edge_dict
        road_edge_id_list = list(road_edge_dict.keys())
        passing_yellow_lane_dict = self.scenario.passing_yellow_lane_dict
        passing_yellow_lane_list = list(passing_yellow_lane_dict.keys())
        if left_boundary_list:
            for l_b in left_boundary_list:
                lb_id = l_b.boundary_feature_id
                lb_type = l_b.boundary_type

                if lb_id in white_lane_id_list :
                    lb_start_idx,_ = find_nearest_point_index(center_x[0],center_y[0], white_lane_dict[lb_id][0],white_lane_dict[lb_id][1]) 
                
                    lb_end_idx,_ = find_nearest_point_index(center_x[-1],center_y[-1], white_lane_dict[lb_id][0],white_lane_dict[lb_id][1])
                    lb_x = white_lane_dict[lb_id][0][lb_start_idx:lb_end_idx]
                    lb_y = white_lane_dict[lb_id][1][lb_start_idx:lb_end_idx]
                    left_b_dict.update({lb_id:[lb_x,lb_y]})
                if lb_id in road_edge_id_list :
                    lb_start_idx,_ = find_nearest_point_index(center_x[0],center_y[0], road_edge_dict[lb_id][0],road_edge_dict[lb_id][1]) 
                
                    lb_end_idx,_ = find_nearest_point_index(center_x[-1],center_y[-1], road_edge_dict[lb_id][0],road_edge_dict[lb_id][1])
                    lb_x = road_edge_dict[lb_id][0][lb_start_idx:lb_end_idx]
                    lb_y = road_edge_dict[lb_id][1][lb_start_idx:lb_end_idx]
                    left_b_dict.update({lb_id:[lb_x,lb_y]})
                if lb_id in passing_yellow_lane_list :
                    lb_start_idx,_ = find_nearest_point_index(center_x[0],center_y[0], passing_yellow_lane_dict[lb_id][0],passing_yellow_lane_dict[lb_id][1]) 
                
                    lb_end_idx,_ = find_nearest_point_index(center_x[-1],center_y[-1], passing_yellow_lane_dict[lb_id][0],passing_yellow_lane_dict[lb_id][1])
                    lb_x = passing_yellow_lane_dict[lb_id][0][lb_start_idx:lb_end_idx]
                    lb_y = passing_yellow_lane_dict[lb_id][1][lb_start_idx:lb_end_idx]
                    
                    left_b_dict.update({lb_id:[lb_x,lb_y]})       
        if right_boundary_list:
            for r_b in right_boundary_list:
                rb_id = r_b.boundary_feature_id
                rb_type = r_b.boundary_type
                if rb_id in white_lane_id_list:
                    rb_start_idx,_ = find_nearest_point_index(center_x[0],center_y[0], white_lane_dict[rb_id][0],white_lane_dict[rb_id][1]) 
                
                    rb_end_idx,_ = find_nearest_point_index(center_x[-1],center_y[-1], white_lane_dict[rb_id][0],white_lane_dict[rb_id][1])
                    # print("rb_start_idx",rb_start_idx)
                    # print("rb_end_idx",rb_end_idx)
                    rb_x = white_lane_dict[rb_id][0][rb_start_idx:rb_end_idx]
                    rb_y = white_lane_dict[rb_id][1][rb_start_idx:rb_end_idx]
                    right_b_dict.update({rb_id:[rb_x,rb_y]})
                if rb_id in road_edge_id_list:
                    rb_start_idx,_ = find_nearest_point_index(center_x[0],center_y[0], road_edge_dict[rb_id][0],road_edge_dict[rb_id][1]) 
                
                    rb_end_idx,_ = find_nearest_point_index(center_x[-1],center_y[-1], road_edge_dict[rb_id][0],road_edge_dict[rb_id][1])
                    rb_x = road_edge_dict[rb_id][0][rb_start_idx:rb_end_idx]
                    rb_y = road_edge_dict[rb_id][1][rb_start_idx:rb_end_idx]
                    right_b_dict.update({rb_id:[rb_x,rb_y]})
                if rb_id in passing_yellow_lane_list:
                    rb_start_idx,_ = find_nearest_point_index(center_x[0],center_y[0], passing_yellow_lane_dict[rb_id][0],passing_yellow_lane_dict[rb_id][1]) 
                
                    rb_end_idx,_ = find_nearest_point_index(center_x[-1],center_y[-1], passing_yellow_lane_dict[rb_id][0],passing_yellow_lane_dict[rb_id][1])
                    rb_x = passing_yellow_lane_dict[rb_id][0][rb_start_idx:rb_end_idx]
                    rb_y = passing_yellow_lane_dict[rb_id][1][rb_start_idx:rb_end_idx]
                    right_b_dict.update({rb_id:[rb_x,rb_y]})
        return left_b_dict,right_b_dict
 
    def get_lane_speed_limit_byID(self,lane_id):
        lane_dict = self.get_center_lane_dict()
        speed_limit_mph = lane_dict[lane_id].speed_limit
        speed_limit_mps = speed_limit_mph * 1609.34 / 3600
        return speed_limit_mps
    
    
    def get_lane_stop_sign_states_byID(self,lane_id):
        stop_sign_list = self.scenario.center_lane_dict[lane_id].stop_sign_list
        stop_point = []
        x,y= self.scenario.center_lane_dict[lane_id].stop_point.x, self.scenario.center_lane_dict[lane_id].stop_point.y
        stop_point.append(x)
        stop_point.append(y)
        return stop_sign_list,stop_point
    @measure_time
    def get_agent_all_time_lane_list(self):
        all_agents_id = self.get_all_car_id_list()
        all_agents = self.scenario.agents_list
        lane = self.scenario.center_lane_dict
        id_dict = self.scenario.agents_id_to_idx_dict
        for track_id in all_agents_id:
            all_time_lane_ids = []
            on_which_lane = []
            track_x_list = self.get_agent_center_x_list_byId(track_id)
            track_y_list = self.get_agent_center_y_list_byId(track_id)
            agent_length = max(self.get_agent_length_list_byId(track_id))
            lane_dist = self.scenario.center_lane_id_dict
            
            ini_lane_ids = self.scenario.agents_list[Id_to_Index(self.scenario.agents_list, track_id)].agent_ini_lane_id_list
            first_app_time = self.scenario.agents_list[Id_to_Index(self.scenario.agents_list, track_id)].first_appear_time
            for i in range(0,first_app_time): #0---first_app_time-1
                all_time_lane_ids.append(None)
            all_time_lane_ids.append(ini_lane_ids)
            ini_in_lane_id = ini_lane_ids[0]
            on_which_lane.append(ini_in_lane_id)
            left_n = []
            right_n = []
            
            if len(ini_lane_ids[1]) != 0:
                left_n = ini_lane_ids[1]
            if len(ini_lane_ids[2]) != 0:
                right_n = ini_lane_ids[2]
            entry_l = ini_lane_ids[3]
            exit_l = ini_lane_ids[4]
            for i in range(first_app_time+1, END_STEP):
                ture_now_lane_id = None
                distance = 10000
                #print(all_time_lane_ids[i-1])
                # if all_time_lane_ids[i-1] is None:
                #     print(i-1,track_id)
                if all_time_lane_ids[i-1] is None:
                    break
                cur_lane = all_time_lane_ids[i-1][0]

                track_x = track_x_list[i]
                track_y = track_y_list[i]
                if (np.isnan(track_x) != True) and (np.isnan(track_y) != True):
                    for p in range(0,min(len(lane_dist[cur_lane][0]),len(lane_dist[cur_lane][1]))):
                        tmp = math.sqrt((lane_dist[cur_lane][0][p]-track_x)**2+(lane_dist[cur_lane][1][p]-track_y)**2)
                        if tmp < distance and tmp < agent_length:
                            distance = tmp
                            ture_now_lane_id = cur_lane
                    if len(left_n) != 0:
                        for nums in range(len(left_n)-1):
                            if left_n[nums] == None:
                                break
                            for p in range(0,min(len(lane_dist[left_n[nums]][0]),len(lane_dist[left_n[nums]][1]))):
                                tmp = math.sqrt((lane_dist[left_n[nums]][0][p]-track_x_list[i])**2+(lane_dist[left_n[nums]][1][p]-track_y_list[i])**2)
                                if  tmp < distance  and tmp < agent_length:
                                    distance = tmp
                                    ture_now_lane_id = left_n[nums]
                    if len(right_n) != 0:
                        for nums in range(len(right_n)):
                            if right_n[nums] == None:
                                break
                            for p in range(0,min(len(lane_dist[right_n[nums]][0]),len(lane_dist[right_n[nums]][1]))):
                                tmp = math.sqrt((lane_dist[right_n[nums]][0][p]-track_x)**2+(lane_dist[right_n[nums]][1][p]-track_y)**2)
                                if tmp < distance  and tmp < agent_length:
                                    distance = tmp
                                    ture_now_lane_id = right_n[nums]
                    if (len(all_time_lane_ids[i-1])!=0):
                        for element in all_time_lane_ids[i-1][4]:
                            if element is not None:
                                # print(element)
                                for p in range(0,min(len(lane_dist[element][0]),len(lane_dist[element][1]))):
                                    tmp = math.sqrt((lane_dist[element][0][p]-track_x)**2+(lane_dist[element][1][p]-track_y)**2)
                                    if tmp < distance and tmp < agent_length:
                                        distance = tmp
                                        ture_now_lane_id = element

                if ture_now_lane_id is not None:
                    if (np.isnan(track_x) == True) and (np.isnan(track_y) == True):

                        all_time_lane_ids.append(None)
                        on_which_lane.append(None)
                    else:
                        on_which_lane.append(ture_now_lane_id)                            
                        all_time_lane_ids.append([ture_now_lane_id,lane[ture_now_lane_id].left_neighbors_id_dict[ture_now_lane_id],lane[ture_now_lane_id].right_neighbors_id_dict[ture_now_lane_id],lane[ture_now_lane_id].entry_lane_id_dict[ture_now_lane_id],lane[ture_now_lane_id].exit_lane_id_dict[ture_now_lane_id]])
                        left_n = lane[ture_now_lane_id].left_neighbors_id_list
                        right_n = lane[ture_now_lane_id].right_neighbors_id_list
                if ture_now_lane_id is None:
                    # if track_id == 2156:
                    #         print("wuyule")
                    all_time_lane_ids.append(None)
                    on_which_lane.append(None)
            
            if len(all_time_lane_ids) < END_STEP:
                 all_time_lane_ids += [None] * (END_STEP - len(all_time_lane_ids))

            if len(on_which_lane) < END_STEP:
                on_which_lane += [None] * (END_STEP - len(on_which_lane))

            self.scenario.agents_list[id_dict[track_id]].all_time_lane_ids = all_time_lane_ids
            self.scenario.agents_list[id_dict[track_id]].on_which_lane = on_which_lane
        return True
    
    def get_agent_all_time_lane_list_byID(self,agent_id):
        all_agents_id = self.scenario.agents_list
        return self.scenario.agents_list[Id_to_Index(all_agents_id,agent_id)].all_time_lane_ids ,self.scenario.agents_list[Id_to_Index(all_agents_id,agent_id)].on_which_lane

    def get_all_time_cars_in_lane(self):
        all_agents_id = self.get_all_car_id_list()
        all_lanes_id = self.scenario.center_lane_id_list
        all_lane = self.scenario.center_lane_list
        for lane_id in all_lanes_id:
            self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].all_time_cars_in_this_lane = [[]for _ in range(END_STEP)]
                
        for agent_id in all_agents_id:
            _,on_which_lane = self.get_agent_all_time_lane_list_byID(agent_id)
            for time_index, lane in enumerate(on_which_lane):
                if lane is not None:
                    self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane)].all_time_cars_in_this_lane[time_index].append(agent_id)
                    
        return True
                    
    def get_all_time_cars_in_lane_byID(self,lane_id):
        all_lanes_id = self.scenario.center_lane_id_list
        return self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].all_time_cars_in_this_lane
    @measure_time
    def get_cars_around_agents(self):
        id_dict = self.scenario.agents_id_to_idx_dict
        all_agents_id = self.get_all_car_id_list()
        all_agents = self.scenario.agents_list
        all_lanes_id = self.scenario.center_lane_id_list
        cars_in_lane = []
        front_car = []
        back_car = []
        left_car = []
        right_car = []
        for agent_id in all_agents_id:
            all_lanes_info,on_which_lane = self.get_agent_all_time_lane_list_byID(agent_id)
            self.scenario.agents_list[id_dict[agent_id]].front_cars = [None for _ in range(END_STEP)]
            self.scenario.agents_list[id_dict[agent_id]].back_cars = [None for _ in range(END_STEP)]
            self.scenario.agents_list[id_dict[agent_id]].left_cars = [None for _ in range(END_STEP)]
            self.scenario.agents_list[id_dict[agent_id]].right_cars = [None for _ in range(END_STEP)]
            self.scenario.agents_list[id_dict[agent_id]].front_cars = [None for _ in range(END_STEP)]
            self.scenario.agents_list[id_dict[agent_id]].back_cars = [None for _ in range(END_STEP)]
            self.scenario.agents_list[id_dict[agent_id]].left_cars = [None for _ in range(END_STEP)]
            self.scenario.agents_list[id_dict[agent_id]].right_cars = [None for _ in range(END_STEP)]
            ego_x_list = self.get_agent_center_x_list_byId(agent_id)
            ego_y_list = self.get_agent_center_y_list_byId(agent_id)
            ego_heading_list = self.get_agent_heading_list_byId(agent_id)
            ego_length_list = self.get_agent_length_list_byId(agent_id)
            for ee in ego_length_list:
                if np.isnan(ee) == False:
                    ego_length = ee
                    break
            for time_index, on_lane_id in enumerate(on_which_lane):
                distance_front = 10000
                distance_back = 10000
                distance_left = 10000
                distance_right = 10000
                ego_x = ego_x_list[time_index]
                ego_y = ego_y_list[time_index]
                ego_heading = ego_heading_list[time_index]
                if (np.isnan(ego_x) != True) and (np.isnan(ego_y) != True) and ( np.isnan(ego_heading) != True) and (on_lane_id is not None) and (np.isnan(ego_length) != True):
                    all_cars_in_this_lane = self.get_all_time_cars_in_lane_byID(on_lane_id)
                    cars_in_lane = all_cars_in_this_lane[time_index] #这条路上所有的车，分两种情况
                    
                    #第一种：这条路上有不止一辆车，那找前后车就只能用heading的朝向了
                    if len(cars_in_lane) >= 2:
                        for other_cars in cars_in_lane:
                            if other_cars != agent_id: #同一条道路上的其他车辆
                                is_hold = self.scenario.agents_list[Id_to_Index(all_agents,other_cars)].is_hold
                                this_x = self.get_agent_center_x_list_byId(other_cars)[time_index]
                                this_y = self.get_agent_center_y_list_byId(other_cars)[time_index]
                                this_heading = self.get_agent_heading_list_byId(other_cars)[time_index]
                                if (np.isnan(this_x) != True) and (np.isnan(this_y) != True) and (np.isnan(this_heading) != True)  :
                                    
                                    
                                    dot_prod1 = (this_x - ego_x) * math.cos(ego_heading) + (this_y - ego_y) * math.sin(ego_heading)
                                    
                                    dot_prod2 = (ego_x - this_x) * math.cos(this_heading) + (ego_y - this_y) * math.sin(this_heading)
                                    
                                    
                                    #flag = judge_front_or_back(ego_x,ego_y,this_x,this_y,ego_heading,this_heading)
                                    
                                    distance = math.sqrt((ego_x - this_x)**2+(ego_y-this_y)**2)
                                    
                                    if dot_prod1 > 0 and (distance < distance_front):
                                        distance_front = distance
                                        self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].front_cars[time_index] = other_cars
                                    
                                    if dot_prod1 < 0 and (distance < distance_back):
                                        distance_back = distance
                                        self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].back_cars[time_index] = other_cars
                                        
                    if self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,on_lane_id)].exit_lane_id_list:
                        front_lane_id = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,on_lane_id)].exit_lane_id_list[0]
                        for element in self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,on_lane_id)].exit_lane_id_list:
                            flag = self.is_this_lane_straight_byID(element)
                            if flag == True:
                                front_lane_id = element
                        cars_in_it = self.get_all_time_cars_in_lane_byID(front_lane_id)[time_index]
                        if cars_in_it:
                            for cars in cars_in_it:
                                is_hold = self.scenario.agents_list[Id_to_Index(all_agents,cars)].is_hold
                                this_x = self.get_agent_center_x_list_byId(cars)[time_index]
                                this_y = self.get_agent_center_y_list_byId(cars)[time_index]
                                if (np.isnan(this_x) != True) and (np.isnan(this_y) != True):
                                    distance = math.sqrt((ego_x - this_x)**2+(ego_y-this_y)**2)
                                    if distance < distance_front:
                                        distance_front = distance
                                        self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].front_cars[time_index] = cars
                                        
                    if self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,on_lane_id)].entry_lane_id_list:
                        back_lane_id = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,on_lane_id)].entry_lane_id_list[0] 
                        for element in self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,on_lane_id)].entry_lane_id_list:
                            flag = self.is_this_lane_straight_byID(element)
                            if flag == True:
                                back_lane_id = element
                        cars_in_it = self.get_all_time_cars_in_lane_byID(back_lane_id)[time_index]
                        if cars_in_it:
                            for cars in cars_in_it:
                                is_hold = self.scenario.agents_list[Id_to_Index(all_agents,cars)].is_hold
                                this_x = self.get_agent_center_x_list_byId(cars)[time_index]
                                this_y = self.get_agent_center_y_list_byId(cars)[time_index]
                                if (np.isnan(this_x) != True) and (np.isnan(this_y) != True)  :
                                    distance = math.sqrt((ego_x - this_x)**2+(ego_y-this_y)**2)
                                    if distance < distance_back:
                                        distance_back = distance
                                        self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].back_cars[time_index] = cars
                                            
                    # 判断左前左后
                    left_neighbors = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,on_lane_id)].left_neighbors_id_list
                    if left_neighbors:
                        for left_neighbor_id in left_neighbors:
                            flag_lf = False
                            flag_lb = False
                            cars_in_it = self.get_all_time_cars_in_lane_byID(left_neighbor_id)[time_index]
                            if cars_in_it:
                                for cars in cars_in_it:
                                    this_x = self.get_agent_center_x_list_byId(cars)[time_index]
                                    this_y = self.get_agent_center_y_list_byId(cars)[time_index]
                                    this_heading = self.get_agent_heading_list_byId(cars)[time_index]
                                    if (np.isnan(this_x) != True) and (np.isnan(this_y) != True):
                                        distance = math.sqrt((ego_x - this_x)**2+(ego_y-this_y)**2)
                                        if distance < distance_left:
                                            distance_left = distance
                                            self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].left_cars[time_index] = cars
                                    if (np.isnan(this_x) != True) and (np.isnan(this_y) != True) and (np.isnan(this_heading) != True):
                                        dot_prod1 = (this_x - ego_x) * math.cos(ego_heading) + (this_y - ego_y) * math.sin(ego_heading)
                                    
                                        dot_prod2 = (ego_x - this_x) * math.cos(this_heading) + (ego_y - this_y) * math.sin(this_heading)
                                        
                                        hh = abs(this_heading-ego_heading)
                                        
                                        if dot_prod1 > 0 and hh < np.pi/2:
                                            self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].left_front_cars[time_index].append(cars)
                                            flag_lf = True #左前没更新
                                        if dot_prod2 > 0 and hh < np.pi/2:
                                            self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].left_back_cars[time_index].append(cars)
                                            flag_lb = True #左后没更新
                            if flag_lf == False:
                                if self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,left_neighbor_id)].exit_lane_id_list:
                                    left_front_neighbor = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,left_neighbor_id)].exit_lane_id_list[0]
                                    cars_in_it = self.get_all_time_cars_in_lane_byID(left_front_neighbor)[time_index]
                                    if cars_in_it:
                                        for cars in cars_in_it:
                                            is_hold = self.scenario.agents_list[Id_to_Index(all_agents,cars)].is_hold
                                            this_x = self.get_agent_center_x_list_byId(cars)[time_index]
                                            this_y = self.get_agent_center_y_list_byId(cars)[time_index]
                                            this_heading = self.get_agent_heading_list_byId(cars)[time_index]
                                            if (np.isnan(this_x) != True) and (np.isnan(this_y) != True) and (np.isnan(this_heading) != True)  :
                                                distance_cal = math.sqrt((this_x-ego_x)**2+(this_y-ego_y)**2)
                                                hh = abs(this_heading-ego_heading)
                                                if distance_cal <= 5*ego_length and hh < np.pi/2:
                                                    self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].left_front_cars[time_index].append(cars)
                            if flag_lb == False:
                                if self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,left_neighbor_id)].entry_lane_id_list:
                                    left_back_neighbor = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,left_neighbor_id)].entry_lane_id_list[0]
                                    cars_in_it = self.get_all_time_cars_in_lane_byID(left_back_neighbor)[time_index]
                                    if cars_in_it:
                                        for cars in cars_in_it:
                                            is_hold = self.scenario.agents_list[Id_to_Index(all_agents,cars)].is_hold
                                            this_x = self.get_agent_center_x_list_byId(cars)[time_index]
                                            this_y = self.get_agent_center_y_list_byId(cars)[time_index]
                                            this_heading = self.get_agent_heading_list_byId(cars)[time_index]
                                            if (np.isnan(this_x) != True) and (np.isnan(this_y) != True) and (np.isnan(this_heading) != True)  :
                                                distance_cal = math.sqrt((this_x-ego_x)**2+(this_y-ego_y)**2)
                                                hh = abs(ego_heading-this_heading)
                                                if distance_cal <= 5*ego_length and hh < np.pi/2:
                                                    self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].left_back_cars[time_index].append(cars)                
                          
                    right_neighbors = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,on_lane_id)].right_neighbors_id_list
                    if right_neighbors:
                        for right_neighbor_id in right_neighbors:
                            cars_in_it = self.get_all_time_cars_in_lane_byID(right_neighbor_id)[time_index]
                            flag_rf = False
                            flag_rb = False
                            if cars_in_it:
                                for cars in cars_in_it:
                                    this_x = self.get_agent_center_x_list_byId(cars)[time_index]
                                    this_y = self.get_agent_center_y_list_byId(cars)[time_index]
                                    this_heading = self.get_agent_heading_list_byId(cars)[time_index]
                                    if (np.isnan(this_x) != True) and (np.isnan(this_y) != True):
                                        distance = math.sqrt((ego_x - this_x)**2+(ego_y-this_y)**2)
                                        if distance < distance_right:
                                            distance_right = distance
                                            self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].right_cars[time_index] = cars
                                    if (np.isnan(this_x) != True) and (np.isnan(this_y) != True) and (np.isnan(this_heading) != True):
                                        dot_prod1 = (this_x - ego_x) * math.cos(ego_heading) + (this_y - ego_y) * math.sin(ego_heading)
                                    
                                        dot_prod2 = (ego_x - this_x) * math.cos(this_heading) + (ego_y - this_y) * math.sin(this_heading)
                                        hh = abs(this_heading-ego_heading)
                                        
                                        if dot_prod2 > 0 and hh < np.pi/2:
                                            self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].right_back_cars[time_index].append(cars)
                                            flag_rf = True
                                        if dot_prod1 > 0 and hh < np.pi/2:
                                            
                                            self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].right_front_cars[time_index].append(cars)
                                            flag_rb = True
                                            
                            if flag_rf == False:
                                if self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,right_neighbor_id)].exit_lane_id_list:
                                    right_front_neighbor = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,right_neighbor_id)].exit_lane_id_list[0]
                                    cars_in_it = self.get_all_time_cars_in_lane_byID(right_front_neighbor)[time_index]
                                    if cars_in_it:
                                        for cars in cars_in_it:
                                            is_hold = self.scenario.agents_list[Id_to_Index(all_agents,cars)].is_hold
                                            this_x = self.get_agent_center_x_list_byId(cars)[time_index]
                                            this_y = self.get_agent_center_y_list_byId(cars)[time_index]
                                            this_heading = self.get_agent_heading_list_byId(cars)[time_index]
                                            if (np.isnan(this_x) != True) and (np.isnan(this_y) != True) and (np.isnan(this_heading) != True)  :
                                                distance_cal = math.sqrt((this_x-ego_x)**2+(this_y-ego_y)**2)
                                                hh = abs(this_heading-ego_heading)
                                                if distance_cal <= 5*ego_length and hh < np.pi/2: 
                                                    self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].right_front_cars[time_index].append(cars)                    
                            if flag_rb == False:
                                if self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,right_neighbor_id)].entry_lane_id_list:
                                    right_back_neighbor = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,right_neighbor_id)].entry_lane_id_list[0]
                                    cars_in_it = self.get_all_time_cars_in_lane_byID(right_back_neighbor)[time_index]
                                    if cars_in_it:
                                        for cars in cars_in_it:
                                            is_hold = self.scenario.agents_list[Id_to_Index(all_agents,cars)].is_hold
                                            this_x = self.get_agent_center_x_list_byId(cars)[time_index]
                                            this_y = self.get_agent_center_y_list_byId(cars)[time_index]
                                            this_heading = self.get_agent_heading_list_byId(cars)[time_index]
                                            
                                            if (np.isnan(this_x) != True) and (np.isnan(this_y) != True) and (np.isnan(this_heading) != True)  :
                                                distance_cal = math.sqrt((this_x-ego_x)**2+(this_y-ego_y)**2)
                                                hh = abs(this_heading-ego_heading)
                                                if distance_cal <= 5*ego_length and hh < np.pi/2:
                                                    self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].right_back_cars[time_index].append(cars) 
        return True  
    def get_around_cars_byID(self,agent_id):
            around_cars = []
            id_dict = self.scenario.agents_id_to_idx_dict
            all_agents = self.scenario.agents_list
            left_front_cars = self.scenario.agents_list[id_dict[agent_id]].left_front_cars
            left_back_cars = self.scenario.agents_list[id_dict[agent_id]].left_back_cars
            right_front_cars = self.scenario.agents_list[id_dict[agent_id]].right_front_cars
            right_back_cars = self.scenario.agents_list[id_dict[agent_id]].right_back_cars
            for time_index in range(END_STEP):
                fc = self.scenario.agents_list[id_dict[agent_id]].front_cars[time_index]
                bc = self.scenario.agents_list[id_dict[agent_id]].back_cars[time_index]
                lc = self.scenario.agents_list[id_dict[agent_id]].left_cars[time_index]
                rc = self.scenario.agents_list[id_dict[agent_id]].right_cars[time_index]
                around_cars.append([fc,bc,lc,rc])
            return around_cars,left_front_cars,left_back_cars,right_front_cars,right_back_cars
    @measure_time    
    def get_distance_to_around_lane(self):
        all_lanes_id = self.scenario.center_lane_id_list
        for lane_id in all_lanes_id:
            
            self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].dis_to_left = {}
            self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].dis_to_right={}
            left_neighbor_info = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].left_neighbors_list
            right_neighbor_info = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].right_neighbors_list
            self_points_x = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].point_x_list
            self_points_y = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].point_y_list
            if right_neighbor_info:
                for right_lane in right_neighbor_info:
                    
                    nei_points_x = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,right_lane.feature_id)].point_x_list
                    nei_points_y = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,right_lane.feature_id)].point_y_list
                    
                    self_start_index = right_lane.self_start_index
                    self_end_index = right_lane.self_end_index
                    neighbor_start_index = right_lane.neighbor_start_index
                    neighbor_end_index = right_lane.neighbor_end_index
                    
                    x1 = self_points_x[self_start_index]
                    x2 = self_points_x[self_end_index]
                    e1 = nei_points_x[neighbor_start_index]
                    e2 = nei_points_x[neighbor_end_index]
                    y1 = self_points_y[self_start_index]
                    y2 = self_points_y[self_end_index]
                    d1 = nei_points_y[neighbor_start_index]
                    d2 = nei_points_y[neighbor_end_index]
                    
                    distance = (math.sqrt((x1-e1)**2+(y1-d1)**2)+math.sqrt((x2-e2)**2+(y2-d2)**2))/2
                    self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].dis_to_right.update({right_lane.feature_id:distance})
            
            if left_neighbor_info:
                for left_lane in left_neighbor_info:
                    
                    nei_points_x = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,left_lane.feature_id)].point_x_list
                    nei_points_y = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,left_lane.feature_id)].point_y_list
                    
                    self_start_index = left_lane.self_start_index
                    self_end_index = left_lane.self_end_index
                    neighbor_start_index = left_lane.neighbor_start_index
                    neighbor_end_index = left_lane.neighbor_end_index
                    
                    x1 = self_points_x[self_start_index]
                    x2 = self_points_x[self_end_index]
                    e1 = nei_points_x[neighbor_start_index]
                    e2 = nei_points_x[neighbor_end_index]
                    y1 = self_points_y[self_start_index]
                    y2 = self_points_y[self_end_index]
                    d1 = nei_points_y[neighbor_start_index]
                    d2 = nei_points_y[neighbor_end_index]
                    
                    distance = (math.sqrt((x1-e1)**2+(y1-d1)**2)+math.sqrt((x2-e2)**2+(y2-d2)**2))/2
                    self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].dis_to_left.update({left_lane.feature_id:distance})
        return True
    
    def get_agent_front_lanes_byID(self,agent_id):
        id_dict = self.scenario.agents_id_to_idx_dict
        lane_dict = self.scenario.center_lane_dict
        _,on_which_lane = self.get_agent_all_time_lane_list_byID(agent_id)
        front_lane_list = []
        if on_which_lane:
            for lane in on_which_lane:
                fl = []
                front_lanes = lane_dict[lane].exit_lane_id_list
                if front_lanes:
                    while len(front_lanes) > 0:
                        exit_l = front_lanes[0]
                        
                        fl.append(exit_l)
                        front_lanes = lane_dict[exit_l].exit_lane_id_list 
                       
                front_lane_list.append(fl)
                
        return front_lane_list
                    
            
    def get_long_lane_points_byID(self,lane_id):
        if lane_id is not None:
            lane_dict = self.scenario.center_lane_dict
            lane_points = []

            front_lanes = lane_dict[lane_id].exit_lane_id_list
            this_lane = lane_dict[lane_id]
            
            point_xlist = this_lane.point_x_list
            point_ylist = this_lane.point_y_list
            while len(front_lanes) > 0:
                exit_l = front_lanes[0]
                point_xlist = point_xlist + lane_dict[exit_l].point_x_list[1:len(lane_dict[exit_l].point_x_list)]
                point_ylist = point_ylist + lane_dict[exit_l].point_y_list[1:len(lane_dict[exit_l].point_y_list)]
                front_lanes = lane_dict[exit_l].exit_lane_id_list
            lane_points.append(point_xlist)
            lane_points.append(point_ylist)
            return np.array(lane_points)
        else:
            return [None,None]
        
        
        
    def get_distance_to_around_lane_byID(self,lane_id):
        all_lanes_id = self.scenario.center_lane_id_list
        return  self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].dis_to_left,self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].dis_to_right
    
    
    @measure_time
    def is_this_lane_straight(self):
        all_lanes_id = self.scenario.center_lane_id_list
        threshold = 0.1
        for lane_id in all_lanes_id:
            point_x = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].point_x_list
            point_y = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].point_y_list
            point_x = np.array(point_x)
            point_y = np.array(point_y)
            if point_x.size > 2 and point_y.size > 2:
                is_straight = is_lane_straight(point_x,point_y , threshold)
            else:
                is_straight = True
            self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].is_straight = is_straight
        return True
    
    def is_this_lane_straight_byID(self,lane_id):
        all_lanes_id = self.scenario.center_lane_id_list
        return self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].is_straight 
    @measure_time
    def is_lane_crossroad(self):
        all_lanes_id = self.scenario.center_lane_id_list
        for lane_id in all_lanes_id:
            exits_lanes = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].exit_lane_id_list
            num_exits = len(exits_lanes)

            if num_exits >= 2:
                for el in exits_lanes:
                    self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,el)].is_lane_crossroad = True
                    left_lane_list = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,el)].left_neighbors_list
                    right_lane_list = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,el)].right_neighbors_list
                    
                    if left_lane_list:
                        for ll in left_lane_list:
                            self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,ll.feature_id)].is_lane_crossroad = True
                            
                    if right_lane_list:
                        for rl in right_lane_list:
                            self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,rl.feature_id)].is_lane_crossroad = True
                self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].is_lane_crossroad = True
                left_lane_list = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].left_neighbors_list
                right_lane_list = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].right_neighbors_list
                
                if left_lane_list:
                    for ll in left_lane_list:
                        self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,ll.feature_id)].is_lane_crossroad = True
                            
                if right_lane_list:
                    for rl in right_lane_list:
                        self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,rl.feature_id)].is_lane_crossroad = True
        
        return True
        
        
    def is_this_lane_crossroad_byID(self,lane_id):
        all_lanes_id = self.scenario.center_lane_id_list
        return self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].is_lane_crossroad
    @measure_time
    def is_this_car_hold(self):
        all_agents =  self.scenario.agents_list
        all_agents_id = self.get_all_car_id_list()
        
        for agent_id in all_agents_id:
            agents_x = self.get_agent_center_x_list_byId(agent_id)
            _, counts = np.unique(agents_x, return_counts=True)
            max_count = np.max(counts)
            agents_y = self.get_agent_center_y_list_byId(agent_id)
            _, counts1 = np.unique(agents_y, return_counts=True)
            max_count1 = np.max(counts1)
            if max_count >= 80 and max_count1 >= 80:
                self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].is_hold = True
        return True
    
    
    def find_independent_car_byID(self,track_id):
        all_agents =  self.scenario.agents_list
        all_agents_id = self.get_all_car_id_list()
        ego_xlist = self.get_agent_center_x_list_byId(track_id)
        ego_ylist = self.get_agent_center_y_list_byId(track_id)
        
        independent_car = [None for _ in range(END_STEP)]
        
        for time_index in range(END_STEP):
            distance_f = 0
            ego_x = ego_xlist[time_index]
            ego_y = ego_ylist[time_index]
            if (np.isnan(ego_x) == False) and (np.isnan(ego_y) == False):
                for agent_id in all_agents_id:
                    this_x = self.get_agent_center_x_list_byId(agent_id)[time_index]
                    this_y = self.get_agent_center_y_list_byId(agent_id)[time_index]
                    
                    if (np.isnan(this_x) == False) and (np.isnan(this_y) == False):
                        
                        distance = math.sqrt((this_x-ego_x)**2+(this_y-ego_y)**2)
                        
                        if distance > distance_f:
                            distance_f = distance
                            independent_car[time_index] = agent_id               
        return independent_car
    def get_lane_points_byID(self,lane_id):
        lane_dist = self.scenario.center_lane_id_dict
        lane = lane_dist[lane_id]
        px = lane[0]
        py = lane[1]
        point_list = []
        point_list.append(px)
        point_list.append(py)
        return point_list
    
    def get_target_lane_points_byID(self,lane_id):
        lane = self.scenario.center_lane_dict
        front_lane_list = lane[lane_id].exit_lane_id_list
        target_lane_dict = {}
        for front_lane in front_lane_list:
            lane_xpoints = lane[lane_id].point_x_list
            lane_ypoints = lane[lane_id].point_y_list
            target_lane_dict.update({front_lane:[lane_xpoints,lane_ypoints]})
        return target_lane_dict
    
    def judge_whether_in_lane_byID(self,agent_id,agent_point,timestep):
        _,on_which_lane = self.get_agent_all_time_lane_list_byID(agent_id)
        if on_which_lane.count(None) == 89:
            on_which_lane[0] = None
        agent_width_list = self.get_agent_width_list_byId(agent_id)
        agent_width = max(agent_width_list)
        road_edge = self.scenario.road_edge_dict
        road_edge_list = list(road_edge.keys())
        flag = True
        for re in road_edge_list:
            _,min_dis = find_min_distance_and_ID(self.scenario.road_edge_dict_tuple[re],[agent_point])
            if min_dis <= agent_width or on_which_lane[timestep] == None:
                flag = False
        return flag
            
    def get_front_agent_inTargetLane_byID(self,agent_id,timestep):
        id_dict = self.scenario.agents_id_to_idx_dict
        _,on_which_lane = self.get_agent_all_time_lane_list_byID(agent_id)
        lane = self.scenario.center_lane_dict
        now_lane = on_which_lane[timestep]
        front_agent_dict = {}
        ego_x = self.get_agent_center_x_list_byId(agent_id)[timestep]
        ego_y = self.get_agent_center_y_list_byId(agent_id)[timestep]
        ego_heading = self.get_agent_heading_list_byId(agent_id)[timestep]
        ego_length = self.get_agent_length_list_byId(agent_id)[timestep]
        if now_lane is not None:
            all_cars_in_this_lane = self.get_all_time_cars_in_lane_byID(now_lane)
            cars_in_lane = all_cars_in_this_lane[timestep] #这条路上所有的车，分两种情况
                    
            #第一种：这条路上有不止一辆车，那找前后车就只能用heading的朝向了
            if len(cars_in_lane) >= 2:
                distance_front = 100000
                for other_cars in cars_in_lane:
                    if other_cars != agent_id: #同一条道路上的其他车辆
                        this_x = self.get_agent_center_x_list_byId(other_cars)[timestep]
                        this_y = self.get_agent_center_y_list_byId(other_cars)[timestep]
                        this_heading = self.get_agent_heading_list_byId(other_cars)[timestep]
                        if (np.isnan(this_x) != True) and (np.isnan(this_y) != True) and (np.isnan(this_heading) != True)  :
                            
                            
                            dot_prod1 = (this_x - ego_x) * math.cos(ego_heading) + (this_y - ego_y) * math.sin(ego_heading)
                            
                            dot_prod2 = (ego_x - this_x) * math.cos(this_heading) + (ego_y - this_y) * math.sin(this_heading)
                                                                    
                            distance = math.sqrt((ego_x - this_x)**2+(ego_y-this_y)**2)
                            
                            if dot_prod1 > 0 and (distance < distance_front):
                                distance_front = distance
                                front_agent_dict.update({now_lane:other_cars})
                            
            target_lane_list = lane[now_lane].exit_lane_id_list
            if target_lane_list:
                for exit_l in target_lane_list:
                    distance_front = 10000
                    cars_in_it = self.get_all_time_cars_in_lane_byID(exit_l)[timestep]
                    if cars_in_it:
                        for cars in cars_in_it:
                            this_x = self.get_agent_center_x_list_byId(cars)[timestep]
                            this_y = self.get_agent_center_y_list_byId(cars)[timestep]
                            if (np.isnan(this_x) != True) and (np.isnan(this_y) != True):
                                distance = math.sqrt((ego_x - this_x)**2+(ego_y-this_y)**2)
                                if distance < distance_front:
                                    distance_front = distance
                                    front_agent_dict.update({now_lane:cars})
                                        
            return front_agent_dict
                    
                    
        
    def get_agents_to_pre(self):
        tcp = self.scenario.tracks_to_pre_list
        track_id_list = []
        agents_list = self.scenario.agents_list
        for i in tcp:
            tcp_idx = i.track_index
            agent_id = agents_list[tcp_idx].agent_id
            track_id_list.append(agent_id)
            # print(agent_id,i.difficulty)
        print("Agents to be predicted: ",track_id_list)
        return track_id_list    
    
    def get_agent_distance_to_neibor_lane_byID(self,track_id,timestep):
        track_x = self.get_agent_center_x_list_byId(track_id)[timestep]
        track_y = self.get_agent_center_y_list_byId(track_id)[timestep]
        center_lanes = self.scenario.center_lane_dict
        _,on_which_lane = self.get_agent_all_time_lane_list_byID(track_id)
        this_lane = on_which_lane[timestep]
        center_lane_id = on_which_lane[timestep]
        left_dis_dict = None
        right_dis_dict = None
        left_lane = []
        right_lane = []
        left_dis_dict = {}
        right_dis_dict = {}
        if center_lane_id is not None:
            left_lane = center_lanes[center_lane_id].left_neighbors_list
            right_lane = center_lanes[center_lane_id].right_neighbors_list
        if left_lane:
            for l_l in left_lane:
                left_x = center_lanes[l_l.feature_id].point_x_list
                left_y = center_lanes[l_l.feature_id].point_y_list
                
                _,distance = find_nearest_point_index(track_x,track_y,left_x,left_y)
                left_dis_dict.update({l_l.feature_id:distance})
        if right_lane:
            for r_l in right_lane:
                right_x = center_lanes[r_l.feature_id].point_x_list
                right_y = center_lanes[r_l.feature_id].point_y_list
                
                _,distance = find_nearest_point_index(track_x,track_y,right_x,right_y)
                right_dis_dict.update({r_l.feature_id:distance})
               
        return left_dis_dict,right_dis_dict
    
    def ground_truth_transfer(self):
        id_list = self.get_all_agents_id()
        id_dict = self.scenario.agents_id_to_idx_dict
        # result_array = np.full(target_length, -1.0, dtype=np.float32)
        gt_trajectory_array = np.zeros((1,128,91,7),dtype = np.float32)-1
        gt_is_valid_array = np.ones((1,128,91), dtype=bool)
        object_type_array = np.zeros((1,128),dtype=np.float32)-1
        idx = 0
        for agent_id in id_list:
            time = 0
            object_type = self.scenario.agents_list[id_dict[agent_id]].states_list[0].agent_type
            for state in self.scenario.agents_list[id_dict[agent_id]].states_list:
                state_array = np.array([state.center_x,state.center_y,state.length,state.width,state.heading,state.velocity_x,state.velocity_y],dtype = np.float32)
                is_valid = state.valid
                gt_trajectory_array[0][idx][time] = state_array
                gt_is_valid_array[0][idx][time] = is_valid
                time += 1
            
            object_type_array[0][idx] = object_type
            idx += 1
        gt_trajectory = tf.convert_to_tensor(gt_trajectory_array)
        gt_is_valid = tf.convert_to_tensor(gt_is_valid_array)
        object_type = tf.convert_to_tensor(object_type_array)
        
        return gt_trajectory,gt_is_valid,object_type
        
    
class Test_Dataloader:
    def __init__(self, data_path, num_scenarios = 1):
        self.scenario_list = process_waymo_training_dataset(data_path, [0])
        self.scenario = self.scenario_list.scenarios_list[0]
        self.get_agents_id_to_idx_dict()
        self.is_this_car_hold()
        self.is_this_lane_straight()
        self.get_all_agents_lane_past()
        self.get_all_time_cars_in_lane_upd()
        # self.get_agent_all_time_lane_list()
        # self.get_all_time_cars_in_lane()
        # self.get_cars_around_agents()
        self.get_distance_to_around_lane()

        self.is_lane_crossroad()

    def get_scenario(self, scenario_index = 0):
        # return self.scenario_list.scenario_list[scenario_index]
        return self.scenario
    def get_agents_id_to_idx_dict(self):
        all_agents = self.scenario.agents_list
        id_dict ={}
        for idx, agent in enumerate(all_agents):
            agent_id = agent.agent_id
            id_dict.update({agent_id:idx})
        self.scenario.agents_id_to_idx_dict = id_dict
        return True
    
    def get_time_index_list(self):
        return self.scenario.timestep_list
    
    def get_scenario_id(self,scenario_index = 0):
        return self.scenario_list.scenario_list[0]
    
    def get_center_lane_dict(self):
        return self.scenario.center_lane_dict
    
    def get_current_time_index(self):
        return self.scenario.current_time_index
    
    def get_all_agents_x_list(self):
        return self.scenario.all_agents_x_list
    
    def get_all_agents_y_list(self):
        return self.scenario.all_agents_y_list
    
    def get_map_feature_list(self):
        return self.scenario.map_features_list

    def get_number_of_map_features(self):
        return self.scenario.number_of_map_features  
    
    def get_passing_double_yellow_list(self):
        return self.scenario.passing_double_yellow_list
        
    def get_broken_double_yellow_list(self):
        return self.scenario.broken_double_yellow_list
    
    def get_broken_single_white_list(self):
        return self.scenario.broken_single_white_list
    
    def get_broken_single_yellow_list(self):
        return self.scenario.broken_single_yellow_list
    
    
    def get_center_lane_list(self):
        return self.scenario.center_lane_list
    
    def get_center_walk_list(self):
        return self.scenario.center_walk_list
    
    def get_road_edge_list(self):
        return self.scenario.road_edge_list
    
    def get_lane_black_list(self):
        return self.scenario.road_edge_list
    
    def get_stop_sign_list(self):
        return self.scenario.stop_sign_list
    
    def get_lane_center_id_dict(self):
        return self.scenario.center_lane_id_dict
    
    def get_solid_double_yellow_list(self):
        return self.scenario.solid_double_yellow_list
    
    def get_solid_double_white_list(self):
        return self.scenario.solid_double_white_list

    def get_solid_single_white_list(self):
        return self.scenario.solid_single_white_list
    
    def get_solid_single_yellow_list(self):
        return self.scenario.solid_single_yellow_list
    
    def get_stop_sign_list(self):
        return self.scenario.stop_sign_list
    
    def get_real_time_list(self,start_time_index , end_time_index):
        return self.scenario.timestep_seconds_list[start_time_index : end_time_index]
    
    def get_all_car_id_list(self):
        return self.scenario.car_id_list
    
    def get_ego_vehicle_states_array(self):
        return self.scenario.agents_list[self.scenario.sdc_track_index].states_array
    
    def get_ego_vehicle_states_list(self):
        return self.scenario.agents_list[self.scenario.sdc_track_index].states_list
    
    def get_ego_vehicle_validation_list(self):
        return self.scenario.agents_list[self.scenario.sdc_track_index].states_mask_list

    def get_all_features_ids(self):
        return self.scenario.all_feature_ids
    
    def get_ego_vehicle_id(self):
        return self.scenario.agents_list[self.scenario.sdc_track_index].agent_id
        
    def get_ego_vehicle_center_x_list(self, start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.center_x for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_center_x = interp_center_position(partial_states)
        return interped_center_x
    
    def get_ego_vehicle_center_y_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.center_y for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_center_y = interp_center_position(partial_states)
        return interped_center_y
   
    def get_ego_vehicle_center_z_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.center_z for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_heading_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.heading for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_heigth_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.height for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_width_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.width for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_length_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.length for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_speed_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.speed for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_v_x_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.velocity_x for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_v_y_list(self,start_time_index = 0, end_time_index = END_STEP):
        partial_states = [state.velocity.y for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        return np.array(partial_states)
    
    def get_ego_vehicle_acc_and_acctheta_list(self,start_time_index = 0, end_time_index = END_STEP):
        len = end_time_index - start_time_index
        v = [state.speed for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        vx = [state.velocity_x for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        vy = [state.velocity_y for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]]
        time = self.scenario.timestep_seconds_list[start_time_index : end_time_index]
        acc = [0] * len
        theta_acc = [0] * len
        for i in range(1, len - 1):
            dt = time[i+1] - time[i]
            acc[i] = (v[i+1] - 2*v[i] + v[i-1]) / (dt**2)
            dx = vx[i+1] - vx[i-1]
            dy = vy[i+1] - vy[i-1]
            theta_acc[i] = math.atan2(dy, dx)
        dt_1 = time[1] - time[0]
        dt_2 = time[2] - time[1]
        acc[0] = (v[1] - v[0]) / dt_1
        acc[1] = (v[2] - v[0]) / (dt_1 + dt_2)
        theta_acc[0] = math.atan2(vy[1], vx[1])
        theta_acc[1] = math.atan2(vy[2] - vy[0], vx[2] - vx[0])
        dt_3 = time[-3] - time[-2]
        dt_4 = time[-2] - time[-1]
        acc[-2] = (v[-1] - v[-3]) / (dt_3 + dt_4)
        acc[-1] = (v[-1] - v[-2]) / dt_4
        theta_acc[-2] = math.atan2(vy[-1] - vy[-3], vx[-1] - vx[-3])
        theta_acc[-1] = math.atan2(vy[-1], vx[-1])
        return np.array(acc), np.array(theta_acc)
        
    def get_ego_vehicle_pos_list(self, start_time_index = 0, end_time_index = END_STEP):
        partial_pos = []
        for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]:
            pos = np.array((state.center_x, state.center_y, state.center_z))
            partial_pos.append(pos)
        partial_pos = np.array(partial_pos)
        return partial_pos
    
    def get_ego_vehicle_shape_list(self, start_time_index = 0, end_time_index = END_STEP):
        partial_shape = []
        for state in self.scenario.agents_list[self.scenario.sdc_track_index].states_list[start_time_index : end_time_index]:
            shape = np.array((state.length, state.width, state.height))
            partial_shape.append(shape)
        partial_shape = np.array(partial_shape)
        return partial_shape    
    
    
    
    def get_front_vehicle_id_list(self, agent_id, start_time = 0, end_time = 0):
        front_vehicle_id_list = []
        id_dict = self.scenario.agents_id_to_idx_dict
        for time_index in range(start_time, end_time):
            front_vehicle_id,_ = find_front_back_vehicle(self.scenario, time_index, self.scenario.agents_list[id_dict[agent_id]].agent_id)
            if front_vehicle_id is None:
                front_vehicle_id = -1
            front_vehicle_id_list.append(front_vehicle_id)
        return front_vehicle_id_list
    
    def get_back_vehicle_id_list(self, agent_id, start_time = 0, end_time = 0):
        back_vehicle_id_list = []
        id_dict = self.scenario.agents_id_to_idx_dict
        for time_index in range(start_time, end_time):
            _,back_vehicle_id = find_front_back_vehicle(self.scenario, time_index, self.scenario.agents_list[id_dict[agent_id]].agent_id)
            if back_vehicle_id is None:
                back_vehicle_id = -1
            back_vehicle_id_list.append(back_vehicle_id)
        return back_vehicle_id_list
    
    
    def get_agent_states_array_byId(self, agent_id):
        id_dict = self.scenario.agents_id_to_idx_dict
        return self.scenario.agents_list[id_dict[agent_id]].states_array
    
    def get_agent_in_lanes_byId(self, agent_id):
        id_dict = self.scenario.agents_id_to_idx_dict
        return self.scenario.agents_list[id_dict[agent_id]].agent_ini_lane_id_list
    
    def get_agent_states_list_byId(self, agent_id):
        id_dict = self.scenario.agents_id_to_idx_dict
        return self.scenario.agents_list[id_dict[agent_id]].states_array
    
    def get_agent_validation_list_byId(self, agent_id):
        id_dict = self.scenario.agents_id_to_idx_dict
        return self.scenario.agents_list[id_dict[agent_id]].states_mask_list
        

        
    def get_agent_center_x_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.center_x for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_center_x = interp_center_position(partial_states)
        for idx,i in enumerate(interped_center_x):
            if  math.isnan(i) or i == 0.0:
                interped_center_x[idx] = None
        return interped_center_x
    
    def get_agent_center_y_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.center_y for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_center_y = interp_center_position(partial_states)
        for idx,i in enumerate(interped_center_y):
            if np.isnan(i) or i == 0.0:
                interped_center_y[idx] = None
        return interped_center_y 
   
    def get_agent_center_z_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.center_z for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_center_z = interp_center_position(partial_states)
        for idx,i in enumerate(interped_center_z):
            if  math.isnan(i) or i == 0.0:
                interped_center_z[idx] = None
        return interped_center_z 
    
    def get_agent_heading_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.heading for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_heading  = interp_center_position(partial_states)
        for idx,i in enumerate(interped_heading):
            if math.isnan(i) or i == 0.0:
                interped_heading[idx] = None
            else:
                interped_heading[idx] = wrap_to_pi(i)
        return interped_heading
    
    def get_agent_height_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.height for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_height = interp_center_position(partial_states)
        for idx,i in enumerate(interped_height):
            if math.isnan(i) or i == 0.0:
                interped_height[idx] = None
        return np.array(partial_states)
    
    def get_agent_width_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.width for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_width = interp_center_position(partial_states)
        for idx,i in enumerate(interped_width):
            if math.isnan(i) or i == 0.0:
                interped_width[idx] = None
        return np.array(partial_states)
    
    def get_agent_length_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.length for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_length = interp_center_position(partial_states)
        for idx,i in enumerate(interped_length):
            if math.isnan(i) or i == 0.0:
                interped_length[idx] = None
        return np.array(partial_states)
    
    def get_agent_speed_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.speed for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_speed  = interp_center_position(partial_states)
        for idx,i in enumerate(interped_speed):
            if math.isnan(i) or i == 0.0:
                interped_speed[idx] = None
        return np.array(interped_speed)
    
    def get_agent_v_x_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.velocity_x for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_vx = interp_center_position(partial_states)
        for idx,i in enumerate(interped_vx):
            if  math.isnan(i) or i == 0.0:
                interped_vx[idx] = None
        return interped_vx 
    
    def get_agent_v_y_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        partial_states = [state.velocity_y for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]]
        partial_states = np.array(partial_states)
        interped_center_vy = interp_center_position(partial_states)
        for idx,i in enumerate(interped_center_vy):
            if math.isnan(i) or i == 0.0:
                interped_center_vy[idx] = None
        return interped_center_vy
    
    def get_agent_acc_and_acctheta_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        id_dict = self.scenario.agents_id_to_idx_dict
        length = end_time_index - start_time_index
        v = self.get_agent_speed_list_byId(agent_id)[start_time_index:end_time_index]
        vx = self.get_agent_v_x_list_byId(agent_id)[start_time_index:end_time_index]
        vy = self.get_agent_v_x_list_byId(agent_id)[start_time_index:end_time_index]
        time = self.scenario.timestep_seconds_list[start_time_index : end_time_index]
        acc = [0] * length
        theta_acc = [0] * length
        for i in range(1, length - 1):
            dt = 0.1
            acc[i] = (v[i]-v[i-1]) / (dt)
            dx = vx[i+1] - vx[i-1]
            dy = vy[i+1] - vy[i-1]
            theta_acc[i] = math.atan2(dy, dx)
        dt_1 = time[1] - time[0]
        dt_2 = time[2] - time[1]
        acc[0] = (v[1] - v[0]) / dt_1
        #acc[1] = (v[2] - v[0]) / (dt_1 + dt_2)
        theta_acc[0] = math.atan2(vy[1], vx[1])
        theta_acc[1] = math.atan2(vy[2] - vy[0], vx[2] - vx[0])
        dt_3 = time[-3] - time[-2]
        dt_4 = time[-2] - time[-1]
        #acc[-2] = (v[-1] - v[-3]) / (dt_3 + dt_4)
        acc[-1] = (v[-1] - v[-2]) / dt_4
        theta_acc[-2] = math.atan2(vy[-1] - vy[-3], vx[-1] - vx[-3])
        theta_acc[-1] = math.atan2(vy[-1], vx[-1])
        return np.array(acc), np.array(theta_acc)
    def get_agent_pos_list_byId(self, agent_id, start_time_index = 0, end_time_index = END_STEP):
        partial_pos = []
        id_dict = self.scenario.agents_id_to_idx_dict
        for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]:
            pos = np.array((state.center_x, state.center_y, state.center_z))
            partial_pos.append(pos)
        partial_pos = np.array(partial_pos)
        return partial_pos
    
    def get_agent_shape_list_byId(self, start_time_index = 0, end_time_index = END_STEP):
        partial_shape = []
        id_dict = self.scenario.agents_id_to_idx_dict
        for state in self.scenario.agents_list[id_dict[agent_id]].states_list[start_time_index : end_time_index]:
            shape = np.array((state.length, state.width, state.height))
            partial_shape.append(shape)
        partial_shape = np.array(partial_shape)
        return partial_shape

    def get_lane_boundary_byID(self,lane_id):
        left_b_dict = {}
        right_b_dict = {}
        lane = self.scenario.center_lane_dict
        center_x = lane[lane_id].point_x_list
        center_y = lane[lane_id].point_y_list
        left_boundary_list = lane[lane_id].left_boundary_list
        right_boundary_list = lane[lane_id].right_boundary_list
        white_lane_dict = self.scenario.white_lane_dict
        white_lane_id_list = list(white_lane_dict.keys())
        road_edge_dict = self.scenario.road_edge_dict
        road_edge_id_list = list(road_edge_dict.keys())
        passing_yellow_lane_dict = self.scenario.passing_yellow_lane_dict
        passing_yellow_lane_list = list(passing_yellow_lane_dict.keys())

        if left_boundary_list:
            for l_b in left_boundary_list:
                lb_id = l_b.boundary_feature_id
                lb_type = l_b.boundary_type

                if lb_id in white_lane_id_list :
                    lb_start_idx,_ = find_nearest_point_index(center_x[0],center_y[0], white_lane_dict[lb_id][0],white_lane_dict[lb_id][1]) 
                
                    lb_end_idx,_ = find_nearest_point_index(center_x[-1],center_y[-1], white_lane_dict[lb_id][0],white_lane_dict[lb_id][1])
                    # print("lb_start_idx",lb_start_idx)
                    # print("lb_end_idx",lb_end_idx)
                    lb_x = white_lane_dict[lb_id][0][lb_start_idx:lb_end_idx]
                    lb_y = white_lane_dict[lb_id][1][lb_start_idx:lb_end_idx]
                    left_b_dict.update({lb_id:[lb_x,lb_y]})
                if lb_id in road_edge_id_list :
                    lb_start_idx,_ = find_nearest_point_index(center_x[0],center_y[0], road_edge_dict[lb_id][0],road_edge_dict[lb_id][1]) 
                
                    lb_end_idx,_ = find_nearest_point_index(center_x[-1],center_y[-1], road_edge_dict[lb_id][0],road_edge_dict[lb_id][1])
                    lb_x = road_edge_dict[lb_id][0][lb_start_idx:lb_end_idx]
                    lb_y = road_edge_dict[lb_id][1][lb_start_idx:lb_end_idx]
                    left_b_dict.update({lb_id:[lb_x,lb_y]})
                if lb_id in passing_yellow_list :

                    lb_start_idx,_ = find_nearest_point_index(center_x[0],center_y[0], passing_yellow_lane_dict[lb_id][0],passing_yellow_lane_dict[lb_id][1]) 
                
                    lb_end_idx,_ = find_nearest_point_index(center_x[-1],center_y[-1], passing_yellow_lane_dict[lb_id][0],passing_yellow_lane_dict[lb_id][1])
                    lb_x = passing_yellow_lane_dict[lb_id][0][lb_start_idx:lb_end_idx]
                    lb_y = passing_yellow_lane_dict[lb_id][1][lb_start_idx:lb_end_idx]
                    left_b_dict.update({lb_id:[lb_x,lb_y]})
                
        if right_boundary_list:
            for r_b in right_boundary_list:
                rb_id = r_b.boundary_feature_id
                rb_type = r_b.boundary_type
                if rb_id in white_lane_id_list:
                    rb_start_idx,_ = find_nearest_point_index(center_x[0],center_y[0], white_lane_dict[rb_id][0],white_lane_dict[rb_id][1]) 
                
                    rb_end_idx,_ = find_nearest_point_index(center_x[-1],center_y[-1], white_lane_dict[rb_id][0],white_lane_dict[rb_id][1])
                    # print("rb_start_idx",rb_start_idx)
                    # print("rb_end_idx",rb_end_idx)
                    rb_x = white_lane_dict[rb_id][0][rb_start_idx:rb_end_idx]
                    rb_y = white_lane_dict[rb_id][1][rb_start_idx:rb_end_idx]
                    right_b_dict.update({rb_id:[rb_x,rb_y]})
                if rb_id in road_edge_id_list:
                    rb_start_idx,_ = find_nearest_point_index(center_x[0],center_y[0], road_edge_dict[rb_id][0],road_edge_dict[rb_id][1]) 
                
                    rb_end_idx,_ = find_nearest_point_index(center_x[-1],center_y[-1], road_edge_dict[rb_id][0],road_edge_dict[rb_id][1])
                    rb_x = road_edge_dict[rb_id][0][rb_start_idx:rb_end_idx]
                    rb_y = road_edge_dict[rb_id][1][rb_start_idx:rb_end_idx]
                    right_b_dict.update({rb_id:[rb_x,rb_y]})
                if rb_id in passing_yellow_lane_list:
                    rb_start_idx,_ = find_nearest_point_index(center_x[0],center_y[0], passing_yellow_lane_dict[rb_id][0],passing_yellow_lane_dict[rb_id][1]) 
                
                    rb_end_idx,_ = find_nearest_point_index(center_x[-1],center_y[-1], passing_yellow_lane_dict[rb_id][0],passing_yellow_lane_dict[rb_id][1])
                    rb_x = passing_yellow_lane_dict[rb_id][0][rb_start_idx:rb_end_idx]
                    rb_y = passing_yellow_lane_dict[rb_id][1][rb_start_idx:rb_end_idx]
                    right_b_dict.update({rb_id:[rb_x,rb_y]})
        return left_b_dict,right_b_dict
 
    def get_lane_speed_limit_byID(self,lane_id):
        lane_dict = self.get_center_lane_dict()
        speed_limit_mph = lane_dict[lane_id].speed_limit
        speed_limit_mps = speed_limit_mph * 1609.34 / 3600
        return speed_limit_mps

    def get_all_time_cars_in_lane_upd(self):
        all_agents_id = self.get_all_car_id_list()
        all_lanes_id = self.scenario.center_lane_id_list
        all_lane = self.scenario.center_lane_list
        for lane_id in all_lanes_id:
            self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].all_time_cars_in_this_lane_upd = [[]for _ in range(END_STEP)]
                
        for agent_id in all_agents_id:
            on_which_lane = self.get_all_agents_lane_past_byID(agent_id)
            for time_index, lane in enumerate(on_which_lane):
                if lane is not None:
                    self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane)].all_time_cars_in_this_lane_upd[time_index].append(agent_id)

    @measure_time    
    def get_distance_to_around_lane(self):
        all_lanes_id = self.scenario.center_lane_id_list
        for lane_id in all_lanes_id:
            
            self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].dis_to_left = {}
            self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].dis_to_right={}
            left_neighbor_info = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].left_neighbors_list
            right_neighbor_info = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].right_neighbors_list
            self_points_x = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].point_x_list
            self_points_y = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].point_y_list
            if right_neighbor_info:
                for right_lane in right_neighbor_info:
                    
                    nei_points_x = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,right_lane.feature_id)].point_x_list
                    nei_points_y = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,right_lane.feature_id)].point_y_list
                    
                    self_start_index = right_lane.self_start_index
                    self_end_index = right_lane.self_end_index
                    neighbor_start_index = right_lane.neighbor_start_index
                    neighbor_end_index = right_lane.neighbor_end_index
                    
                    x1 = self_points_x[self_start_index]
                    x2 = self_points_x[self_end_index]
                    e1 = nei_points_x[neighbor_start_index]
                    e2 = nei_points_x[neighbor_end_index]
                    y1 = self_points_y[self_start_index]
                    y2 = self_points_y[self_end_index]
                    d1 = nei_points_y[neighbor_start_index]
                    d2 = nei_points_y[neighbor_end_index]
                    
                    distance = (math.sqrt((x1-e1)**2+(y1-d1)**2)+math.sqrt((x2-e2)**2+(y2-d2)**2))/2
                    self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].dis_to_right.update({right_lane.feature_id:distance})
            
            if left_neighbor_info:
                for left_lane in left_neighbor_info:
                    
                    nei_points_x = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,left_lane.feature_id)].point_x_list
                    nei_points_y = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,left_lane.feature_id)].point_y_list
                    
                    self_start_index = left_lane.self_start_index
                    self_end_index = left_lane.self_end_index
                    neighbor_start_index = left_lane.neighbor_start_index
                    neighbor_end_index = left_lane.neighbor_end_index
                    
                    x1 = self_points_x[self_start_index]
                    x2 = self_points_x[self_end_index]
                    e1 = nei_points_x[neighbor_start_index]
                    e2 = nei_points_x[neighbor_end_index]
                    y1 = self_points_y[self_start_index]
                    y2 = self_points_y[self_end_index]
                    d1 = nei_points_y[neighbor_start_index]
                    d2 = nei_points_y[neighbor_end_index]
                    
                    distance = (math.sqrt((x1-e1)**2+(y1-d1)**2)+math.sqrt((x2-e2)**2+(y2-d2)**2))/2
                    self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].dis_to_left.update({left_lane.feature_id:distance})
        return True
    
    def get_distance_to_around_lane_byID(self,lane_id):
        all_lanes_id = self.scenario.center_lane_id_list
        return  self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].dis_to_left,self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].dis_to_right
    
    
    @measure_time
    def is_this_lane_straight(self):
        all_lanes_id = self.scenario.center_lane_id_list
        threshold = 0.1
        for lane_id in all_lanes_id:
            point_x = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].point_x_list
            point_y = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].point_y_list
            point_x = np.array(point_x)
            point_y = np.array(point_y)
            if point_x.size > 2 and point_y.size > 2:
                is_straight = is_lane_straight(point_x,point_y , threshold)
            else:
                is_straight = True
            self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].is_straight = is_straight
        return True
    
    def is_this_lane_straight_byID(self,lane_id):
        all_lanes_id = self.scenario.center_lane_id_list
        return self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].is_straight 
    @measure_time
    def is_lane_crossroad(self):
        all_lanes_id = self.scenario.center_lane_id_list
        for lane_id in all_lanes_id:
            exits_lanes = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].exit_lane_id_list
            num_exits = len(exits_lanes)

            if num_exits >= 2:
                for el in exits_lanes:
                    self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,el)].is_lane_crossroad = True
                    left_lane_list = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,el)].left_neighbors_list
                    right_lane_list = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,el)].right_neighbors_list
                    
                    if left_lane_list:
                        for ll in left_lane_list:
                            self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,ll.feature_id)].is_lane_crossroad = True
                            
                    if right_lane_list:
                        for rl in right_lane_list:
                            self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,rl.feature_id)].is_lane_crossroad = True
                self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].is_lane_crossroad = True
                left_lane_list = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].left_neighbors_list
                right_lane_list = self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].right_neighbors_list
                
                if left_lane_list:
                    for ll in left_lane_list:
                        self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,ll.feature_id)].is_lane_crossroad = True
                            
                if right_lane_list:
                    for rl in right_lane_list:
                        self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,rl.feature_id)].is_lane_crossroad = True
        
        return True
        
        
    def is_this_lane_crossroad_byID(self,lane_id):
        all_lanes_id = self.scenario.center_lane_id_list
        return self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].is_lane_crossroad
    @measure_time
    def is_this_car_hold(self):
        all_agents =  self.scenario.agents_list
        all_agents_id = self.get_all_car_id_list()
        
        for agent_id in all_agents_id:
            agents_x = self.get_agent_center_x_list_byId(agent_id)
            _, counts = np.unique(agents_x, return_counts=True)
            max_count = np.max(counts)
            agents_y = self.get_agent_center_y_list_byId(agent_id)
            _, counts1 = np.unique(agents_y, return_counts=True)
            max_count1 = np.max(counts1)
            if max_count >= 9 and max_count1 >= 9:
                self.scenario.agents_list[Id_to_Index(all_agents,agent_id)].is_hold = True
        return True
    
    def get_lane_points_byID(self,lane_id):
        lane_dist = self.scenario.center_lane_id_dict
        lane = lane_dist[lane_id]
        px = lane[0]
        py = lane[1]
        point_list = []
        point_list.append(px)
        point_list.append(py)
        return point_list
            
    def get_agents_to_pre(self):
        tcp = self.scenario.tracks_to_pre_list
        track_id_list = []
        agents_list = self.scenario.agents_list
        for i in tcp:
            tcp_idx = i.track_index
            agent_id = agents_list[tcp_idx].agent_id
            track_id_list.append(agent_id)
            # print(agent_id,i.difficulty)
        return track_id_list    
    
    def get_agent_distance_to_neibor_lane_byID(self,track_id,timestep):
        track_x = self.get_agent_center_x_list_byId(track_id)[timestep]
        track_y = self.get_agent_center_y_list_byId(track_id)[timestep]
        center_lanes = self.scenario.center_lane_dict
        _,on_which_lane = self.get_agent_all_time_lane_list_byID(track_id)
        this_lane = on_which_lane[timestep]
        center_lane_id = on_which_lane[timestep]
        left_lane = center_lanes[center_lane_id].left_neighbors_list
        right_lane = center_lanes[center_lane_id].right_neighbors_list
        left_dis_dict = {}
        right_dis_dict = {}
        if left_lane:
            for l_l in left_lane:
                left_x = center_lanes[l_l.feature_id].point_x_list
                left_y = center_lanes[l_l.feature_id].point_y_list
                
                _,distance = find_nearest_point_index(track_x,track_y,left_x,left_y)
                left_dis_dict.update({l_l.feature_id:distance})
        if right_lane:
            for r_l in right_lane:
                right_x = center_lanes[r_l.feature_id].point_x_list
                right_y = center_lanes[r_l.feature_id].point_y_list
                
                _,distance = find_nearest_point_index(track_x,track_y,right_x,right_y)
                right_dis_dict.update({r_l.feature_id:distance})
                
        return left_dis_dict,right_dis_dict
    
    @measure_time
    def get_all_agents_lane_past(self):
        all_agents_id = self.get_all_car_id_list()
        #all_agents = self.scenario.agents_list
        lane = self.scenario.center_lane_dict
        lane_id_list = list(lane.keys())
        id_dict = self.scenario.agents_id_to_idx_dict
        for track_id in all_agents_id:
            on_which_lane_upd = []
            track_x_list = self.get_agent_center_x_list_byId(track_id)
            track_y_list = self.get_agent_center_y_list_byId(track_id)
            #self.scenario.agents_list[id_dict[track_id]].is_on_lane_state = []
            for tp in range(11):
                on_lane_id = None
                tx = track_x_list[tp]
                ty = track_y_list[tp]
                track_length = self.get_agent_length_list_byId(track_id)[tp]
                if (np.isnan(tx) == False) and (np.isnan(ty) == False):
                    position = (tx,ty)
                    distance_dict = {}
                    for lane_id in lane_id_list:
                        lane_x = lane[lane_id].point_x_list
                        lane_y = lane[lane_id].point_y_list
                        mean_distance = get_Manhattan_distance(position,lane_x,lane_y)
                        distance_dict.update({lane_id:mean_distance})
                    sorted_items = sorted(distance_dict.items(), key=lambda item: item[1])
                    tp5_sorted = sorted_items[:5]
                    mdis = 1000000
                    if tp == 0 or on_which_lane_upd[tp-1] == None:
                        for lane_id,dis in tp5_sorted:
                            lane_x = lane[lane_id].point_x_list
                            lane_y = lane[lane_id].point_y_list
                            mean_distance = get_Manhattan_distance(position,lane_x,lane_y)
                            idx,min_distance = find_nearest_point_index(tx,ty,lane_x,lane_y)
                            if min_distance < mdis and min_distance < track_length:

                                on_lane_id = lane_id
                                mdis = min_distance

                                
                        if on_lane_id != None:
                            self.scenario.agents_list[id_dict[track_id]].is_on_lane_state.append(True)
                            
                            left_lane_list = lane[on_lane_id].left_neighbors_id_list
                            right_lane_list = lane[on_lane_id].right_neighbors_id_list
                            exit_lane_list = lane[on_lane_id].exit_lane_id_list
                        else:
                            self.scenario.agents_list[id_dict[track_id]].is_on_lane_state.append(False)

                        on_which_lane_upd.append(on_lane_id) 
                    else:   
                        for lane_id,dis in tp5_sorted:
                            flag = False
                            lane_x = lane[lane_id].point_x_list
                            lane_y = lane[lane_id].point_y_list
                            mean_distance = get_Manhattan_distance(position,lane_x,lane_y)
                            idx,min_distance = find_nearest_point_index(tx,ty,lane_x,lane_y)
                            if (lane_id in left_lane_list) or (lane_id in right_lane_list) or (lane_id in exit_lane_list) or (lane_id == on_which_lane_upd[tp-1]) : 
                                flag = True
                            if min_distance < mdis and (flag == True) and (min_distance < track_length):
                                on_lane_id = lane_id
                                mdis = min_distance
                            if on_lane_id == None and on_which_lane_upd[tp-1] != None:
                                on_lane_id = on_which_lane_upd[tp-1]
                            if on_lane_id == None and on_which_lane_upd[tp-1] == None:
                                on_lane_id == lane_id
                                mdis = min_distance
                        if on_lane_id != None:
                            self.scenario.agents_list[id_dict[track_id]].is_on_lane_state.append(True)
                            
                            left_lane_list = lane[on_lane_id].left_neighbors_id_list
                            right_lane_list = lane[on_lane_id].right_neighbors_id_list
                            exit_lane_list = lane[on_lane_id].exit_lane_id_list
                        if on_lane_id == None:

                            self.scenario.agents_list[id_dict[track_id]].is_on_lane_state.append(False)

                        on_which_lane_upd.append(on_lane_id)
                else:
                    self.scenario.agents_list[id_dict[track_id]].is_on_lane_state.append(False)
                    on_which_lane_upd.append(None)
            self.scenario.agents_list[id_dict[track_id]].on_which_lane_upd = on_which_lane_upd
        return True
    def get_all_agents_lane_past_byID(self,track_id):
        id_dict = self.scenario.agents_id_to_idx_dict
        on_which_lane_upd = self.scenario.agents_list[id_dict[track_id]].on_which_lane_upd 
        most_lane , most_num = most_common_element_and_count(on_which_lane_upd)
        if (most_num == 9) and  (on_which_lane_upd[-1] == most_lane):
            on_which_lane_upd = [most_lane] * 10
        return  self.scenario.agents_list[id_dict[track_id]].on_which_lane_upd
    
    # def get_all_agents_lane_updated(self,position):
    #     track_x = postion[0]
        
    def get_position_on_which_lane_byID(self,position):
        lane = self.scenario.center_lane_dict
        lane_id_list = list(lane.keys())
        tx = position[0]
        ty = position[1]
        if (np.isnan(tx) == False) and (np.isnan(ty) == False):
            position = (tx,ty)
            distance_dict = {}
            for lane_id in lane_id_list:
                lane_x = lane[lane_id].point_x_list
                lane_y = lane[lane_id].point_y_list
                mean_distance = get_Manhattan_distance(position,lane_x,lane_y)
                distance_dict.update({lane_id:mean_distance})
            sorted_items = sorted(distance_dict.items(), key=lambda item: item[1])
            tp5_sorted = sorted_items[:5]
            mdis = 1000000
            for lane_id,dis in tp5_sorted:
                lane_x = lane[lane_id].point_x_list
                lane_y = lane[lane_id].point_y_list
                mean_distance = get_Manhattan_distance(position,lane_x,lane_y)
                idx,min_distance = find_nearest_point_index(tx,ty,lane_x,lane_y)
                if min_distance < mdis:
                    on_lane_id = lane_id
                    mdis = min_distance
        return on_lane_id   
    
    def get_all_time_cars_in_lane_upd_byID(self,lane_id):
        all_lanes_id = self.scenario.center_lane_id_list
        return self.scenario.center_lane_list[lane_id_to_index(all_lanes_id,lane_id)].all_time_cars_in_this_lane_upd
    
    
    def get_lane_stop_sign_states_byID(self,lane_id):
        stop_sign_list = self.scenario.center_lane_dict[lane_id].stop_sign_list
        stop_point = []
        x,y= self.scenario.center_lane_dict[lane_id].stop_point.x, self.scenario.center_lane_dict[lane_id].stop_point.y
        stop_point.append(x)
        stop_point.append(y)
        return stop_sign_list,stop_point
    
    def get_is_agent_on_lane_list_byID(self,track_id):
        id_dict = self.scenario.agents_id_to_idx_dict
        return  self.scenario.agents_list[id_dict[track_id]].is_on_lane_state
    
    def get_center_lane_id_list(self):
        return self.scenario.center_lane_id_list
    
    def get_target_lane_points_byID(self,lane_id):
        lane = self.scenario.center_lane_dict
        front_lane_list = lane[lane_id].exit_lane_id_list
        target_lane_dict = {}
        for front_lane in front_lane_list:
            lane_xpoints = lane[lane_id].point_x_list
            lane_ypoints = lane[lane_id].point_y_list
            target_lane_dict.update({front_lane:[lane_xpoints,lane_ypoints]})
        return target_lane_dict
    
    def get_front_agent_inTargetLane_byID(self,agent_id,timestep):
        id_dict = self.scenario.agents_id_to_idx_dict
        _,on_which_lane = self.get_agent_all_time_lane_list_byID(agent_id)
        lane = self.scenario.center_lane_dict
        now_lane = on_which_lane[timestep]
        front_agent_dict = {}
        ego_x = self.get_agent_center_x_list_byId(agent_id)[timestep]
        ego_y = self.get_agent_center_y_list_byId(agent_id)[timestep]
        ego_heading = self.get_agent_heading_list_byId(agent_id)[timestep]
        ego_length = self.get_agent_length_list_byId(agent_id)[timestep]
        if now_lane is not None:
            all_cars_in_this_lane = self.get_all_time_cars_in_lane_byID(now_lane)
            cars_in_lane = all_cars_in_this_lane[timestep] #这条路上所有的车，分两种情况
                    
            #第一种：这条路上有不止一辆车，那找前后车就只能用heading的朝向了
            if len(cars_in_lane) >= 2:
                distance_front = 100000
                for other_cars in cars_in_lane:
                    if other_cars != agent_id: #同一条道路上的其他车辆
                        this_x = self.get_agent_center_x_list_byId(other_cars)[timestep]
                        this_y = self.get_agent_center_y_list_byId(other_cars)[timestep]
                        this_heading = self.get_agent_heading_list_byId(other_cars)[timestep]
                        if (np.isnan(this_x) != True) and (np.isnan(this_y) != True) and (np.isnan(this_heading) != True)  :
                            
                            
                            dot_prod1 = (this_x - ego_x) * math.cos(ego_heading) + (this_y - ego_y) * math.sin(ego_heading)
                            
                            dot_prod2 = (ego_x - this_x) * math.cos(this_heading) + (ego_y - this_y) * math.sin(this_heading)
                                                                    
                            distance = math.sqrt((ego_x - this_x)**2+(ego_y-this_y)**2)
                            
                            if dot_prod1 > 0 and (distance < distance_front):
                                distance_front = distance
                                front_agent_dict.update({now_lane:other_cars})
                            
            target_lane_list = lane[now_lane].exit_lane_id_list
            if target_lane_list:
                for exit_l in target_lane_list:
                    distance_front = 10000
                    cars_in_it = self.get_all_time_cars_in_lane_byID(exit_l)[timestep]
                    if cars_in_it:
                        for cars in cars_in_it:
                            this_x = self.get_agent_center_x_list_byId(cars)[timestep]
                            this_y = self.get_agent_center_y_list_byId(cars)[timestep]
                            if (np.isnan(this_x) != True) and (np.isnan(this_y) != True):
                                distance = math.sqrt((ego_x - this_x)**2+(ego_y-this_y)**2)
                                if distance < distance_front:
                                    distance_front = distance
                                    front_agent_dict.update({now_lane:cars})
                                        
            return front_agent_dict
    def get_long_lane_points_byID(self,lane_id):
        lane_dict = self.scenario.center_lane_dict
        lane_points = []
        front_lanes = lane_dict[lane_id].exit_lane_id_list
        this_lane = lane_dict[lane_id]
        
        point_xlist = this_lane.point_x_list
        point_ylist = this_lane.point_y_list
        while len(front_lanes) > 0:
            exit_l = front_lanes[0]
            point_xlist = point_xlist + lane_dict[exit_l].point_x_list[1:len(lane_dict[exit_l].point_x_list)-1]
            point_ylist = point_ylist + lane_dict[exit_l].point_y_list[1:len(lane_dict[exit_l].point_y_list)-1]
            front_lanes = lane_dict[exit_l].exit_lane_id_list
        lane_points.append(point_xlist)
        lane_points.append(point_ylist)
        return lane_points